pinocchio  2.5.0
A fast and flexible implementation of Rigid Body Dynamics algorithms and their analytical derivatives
Build reduced model

This example shows how to build a reduced robot model from an existing URDF model by fixing the desired joints at a specified position.

Python

import pinocchio as pin
import numpy as np
from os.path import*
# Goal: Build a reduced model from an existing URDF model by fixing the desired joints at a specified position.
# Load UR robot arm
# This path refers to Pinocchio source code but you can define your own directory here.
pinocchio_model_dir = join(dirname(os.dirname(str(abspath(__file__)))), "models")
model_path = pinocchio_model_dir + '/others/robots'
mesh_dir = model_path
# You should change here to set up your own URDF file
urdf_filename = model_path + '/ur_description/urdf/ur5_robot.urdf'
model, collision_model, visual_model = pin.buildModelsFromUrdf(urdf_filename, mesh_dir)
# Check dimensions of the original model
print('standard model: dim=' + str(len(model.joints)))
for jn in model.joints:
print(jn)
# Create a list of joints to lock
jointsToLock = ['wrist_1_joint', 'wrist_2_joint', 'wrist_3_joint']
# Get the ID of all existing joints
jointsToLockIDs = []
for jn in jointsToLock:
if model.existJointName(jn):
jointsToLockIDs.append(model.getJointId(jn))
else:
print('Warning: joint ' + str(jn) + ' does not belong to the model!')
# Set initial position of both fixed and revoulte joints
initialJointConfig = np.array([0,0,0, # shoulder and elbow
1,1,1]) # gripper)
# Option 1: Build the reduced model including the geometric model for proper displaying of the robot
model_reduced, visual_model_reduced = pin.buildReducedModel(model, visual_model, jointsToLockIDs, initialJointConfig)
# Option 2: Only build the reduced model in case no display needed:
# model_reduced = pin.buildReducedModel(model, jointsToLockIDs, initialJointConfig)
# Check dimensions of the reduced model
print('reduced model: dim=' + str(len(model_reduced.joints)))
for jn in model_reduced.joints:
print(jn)

C++

#include "pinocchio/parsers/urdf.hpp"
#include "pinocchio/algorithm/joint-configuration.hpp"
#include "pinocchio/algorithm/model.hpp"
#include <iostream>
#include <algorithm>
// PINOCCHIO_MODEL_DIR is defined by the CMake but you can define your own directory here.
#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)
{
using namespace pinocchio;
// You should change here to set up your own URDF file or just pass it as an argument of this example.
const std::string urdf_filename = (argc<=1) ? PINOCCHIO_MODEL_DIR + std::string("/others/robots/ur_description/urdf/ur5_robot.urdf") : argv[1];
// Load the urdf model
Model model;
pinocchio::urdf::buildModel(urdf_filename,model);
// Create a list of joint to lock
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"); // It can be in the wrong order
list_of_joints_to_lock_by_name.push_back("wrist_2_joint");
list_of_joints_to_lock_by_name.push_back("blabla"); // Joint not in the model
// Print the list of joints to remove + retrieve the joint id
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;
if(model.existJointName(joint_name)) // do not consider joint that are not in the model
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;
}
// Sample any random configuration
Eigen::VectorXd q_rand = randomConfiguration(model);
// std::cout << "q_rand: " << q_rand.transpose() << std::endl;
// But should be also a neutral configuration
Eigen::VectorXd q_neutral= neutral(model);
PINOCCHIO_UNUSED_VARIABLE(q_neutral);
// std::cout << "q_neutral: " << q_neutral.transpose() << std::endl;
std::cout << "\n\nFIRST CASE: BUILD A REDUCED MODEL FROM A LIST OF JOINT TO LOCK" << std::endl;
// Build the reduced model from the list of lock joints
Model reduced_model = buildReducedModel(model,list_of_joints_to_lock_by_id,q_rand);
// Print the list of joints in the original model
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;
// Print the list of joints in the reduced model
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;
// The same thing, but this time with an input list of joint to keep
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;
if(model.existJointName(joint_name))
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";
}
// Transform the list into a list of joints to lock
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);
}
}
// Build the reduced model from the list of lock joints
Model reduced_model2 = buildReducedModel(model,list_of_joints_to_lock_by_id,q_rand);
// Print the list of joints in the second reduced model
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;
}
pinocchio::ModelTpl::getJointId
JointIndex getJointId(const std::string &name) const
Return the index of a joint given by its name.
pinocchio::ModelTpl::joints
JointModelVector joints
Model of joint i, encapsulated in a JointModelAccessor.
Definition: model.hpp:96
pinocchio::buildReducedModel
void buildReducedModel(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, std::vector< JointIndex > list_of_joints_to_lock, const Eigen::MatrixBase< ConfigVectorType > &reference_configuration, ModelTpl< Scalar, Options, JointCollectionTpl > &reduced_model)
Build a reduced model from a given input model and a list of joint to lock.
pinocchio::randomConfiguration
void randomConfiguration(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, const Eigen::MatrixBase< ConfigVectorIn1 > &lowerLimits, const Eigen::MatrixBase< ConfigVectorIn2 > &upperLimits, const Eigen::MatrixBase< ReturnType > &qout)
Generate a configuration vector uniformly sampled among provided limits.
Definition: joint-configuration.hpp:224
pinocchio::ModelTpl::names
std::vector< std::string > names
Name of joint i
Definition: model.hpp:114
pinocchio::urdf::buildModel
ModelTpl< Scalar, Options, JointCollectionTpl > & buildModel(const std::string &filename, const typename ModelTpl< Scalar, Options, JointCollectionTpl >::JointModel &rootJoint, ModelTpl< Scalar, Options, JointCollectionTpl > &model, const bool verbose=false)
Build the model from a URDF file with a particular joint as root of the model tree inside the model g...
pinocchio::neutral
void neutral(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, const Eigen::MatrixBase< ReturnType > &qout)
Return the neutral configuration element related to the model configuration space.
Definition: joint-configuration.hpp:257
pinocchio::ModelTpl::existJointName
bool existJointName(const std::string &name) const
Check if a joint given by its name exists.
pinocchio::ModelTpl
Definition: fwd.hpp:23
pinocchio
Main pinocchio namespace.
Definition: treeview.dox:24