This example shows how to build a reduced robot model from an existing URDF model by fixing the desired joints at a specified position.
1 import pinocchio
as pin
9 pinocchio_model_dir = join(dirname(os.dirname(str(abspath(__file__)))),
"models")
10 model_path = pinocchio_model_dir +
'/others/robots' 13 urdf_filename = model_path +
'/ur_description/urdf/ur5_robot.urdf' 14 model, collision_model, visual_model = pin.buildModelsFromUrdf(urdf_filename, mesh_dir)
17 print(
'standard model: dim=' + str(len(model.joints)))
18 for jn
in model.joints:
22 jointsToLock = [
'wrist_1_joint',
'wrist_2_joint',
'wrist_3_joint']
26 for jn
in jointsToLock:
27 if model.existJointName(jn):
28 jointsToLockIDs.append(model.getJointId(jn))
30 print(
'Warning: joint ' + str(jn) +
' does not belong to the model!')
33 initialJointConfig = np.array([0,0,0,
37 model_reduced, visual_model_reduced = pin.buildReducedModel(model, visual_model, jointsToLockIDs, initialJointConfig)
43 print(
'reduced model: dim=' + str(len(model_reduced.joints)))
44 for jn
in model_reduced.joints:
#include "pinocchio/parsers/urdf.hpp"
#include "pinocchio/algorithm/joint-configuration.hpp"
#include "pinocchio/algorithm/model.hpp"
#include <iostream>
#include <algorithm>
#ifndef PINOCCHIO_MODEL_DIR
#define PINOCCHIO_MODEL_DIR "path_to_the_model_dir"
#endif
template<typename T>
bool is_in_vector(const std::vector<T> & vector, const T & elt)
{
return vector.end() != std::find(vector.begin(),vector.end(),elt);
}
int main(int argc, char ** argv)
{
const std::string urdf_filename = (argc<=1) ? PINOCCHIO_MODEL_DIR + std::string("/others/robots/ur_description/urdf/ur5_robot.urdf") : argv[1];
std::vector<std::string> list_of_joints_to_lock_by_name;
list_of_joints_to_lock_by_name.push_back("elbow_joint");
list_of_joints_to_lock_by_name.push_back("wrist_3_joint");
list_of_joints_to_lock_by_name.push_back("wrist_2_joint");
list_of_joints_to_lock_by_name.push_back("blabla");
std::vector<JointIndex> list_of_joints_to_lock_by_id;
for(std::vector<std::string>::const_iterator it = list_of_joints_to_lock_by_name.begin();
it != list_of_joints_to_lock_by_name.end(); ++it)
{
const std::string & joint_name = *it;
list_of_joints_to_lock_by_id.push_back(model.
getJointId(joint_name));
else
std::cout << "joint: " << joint_name << " does not belong to the model" << std::endl;
}
Eigen::VectorXd q_neutral=
neutral(model);
PINOCCHIO_UNUSED_VARIABLE(q_neutral);
std::cout << "\n\nFIRST CASE: BUILD A REDUCED MODEL FROM A LIST OF JOINT TO LOCK" << std::endl;
std::cout << "List of joints in the original model:" << std::endl;
for(JointIndex joint_id = 1; joint_id < model.
joints.size(); ++joint_id)
std::cout <<
"\t- " << model.
names[joint_id] << std::endl;
std::cout << "List of joints in the reduced model:" << std::endl;
for(JointIndex joint_id = 1; joint_id < reduced_model.
joints.size(); ++joint_id)
std::cout <<
"\t- " << reduced_model.
names[joint_id] << std::endl;
std::cout << "\n\nSECOND CASE: BUILD A REDUCED MODEL FROM A LIST OF JOINT TO KEEP UNLOCKED" << std::endl;
std::vector<std::string> list_of_joints_to_keep_unlocked_by_name;
list_of_joints_to_keep_unlocked_by_name.push_back("shoulder_pan_joint");
list_of_joints_to_keep_unlocked_by_name.push_back("shoulder_lift_joint");
list_of_joints_to_keep_unlocked_by_name.push_back("wrist_1_joint");
std::vector<JointIndex> list_of_joints_to_keep_unlocked_by_id;
for(std::vector<std::string>::const_iterator it = list_of_joints_to_keep_unlocked_by_name.begin();
it != list_of_joints_to_keep_unlocked_by_name.end(); ++it)
{
const std::string & joint_name = *it;
list_of_joints_to_keep_unlocked_by_id.push_back(model.
getJointId(joint_name));
else
std::cout << "joint: " << joint_name << " does not belong to the model";
}
list_of_joints_to_lock_by_id.clear();
for(JointIndex joint_id = 1; joint_id < model.
joints.size(); ++joint_id)
{
const std::string joint_name = model.
names[joint_id];
if(is_in_vector(list_of_joints_to_keep_unlocked_by_name,joint_name))
continue;
else
{
list_of_joints_to_lock_by_id.push_back(joint_id);
}
}
std::cout << "List of joints in the second reduced model:" << std::endl;
for(JointIndex joint_id = 1; joint_id < reduced_model2.
joints.size(); ++joint_id)
std::cout <<
"\t- " << reduced_model2.
names[joint_id] << std::endl;
}