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 os
from pathlib import Path

import numpy as np
import pinocchio as pin

# 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.
model_path = Path(os.environ.get("EXAMPLE_ROBOT_DATA_MODEL_DIR"))
mesh_dir = model_path.parent.parent
# 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)
print("-" * 30)

# 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: Only build the reduced model in case no display needed:
model_reduced = pin.buildReducedModel(model, jointsToLockIDs, initialJointConfig)

# Option 2: 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 3: Build the reduced model including multiple geometric models (for example:
# visuals, collision).
geom_models = [visual_model, collision_model]
model_reduced, geometric_models_reduced = pin.buildReducedModel(
model,
list_of_geom_models=geom_models,
list_of_joints_to_lock=jointsToLockIDs,
reference_configuration=initialJointConfig,
)
# geometric_models_reduced is a list, ordered as the passed variable "geom_models" so:
visual_model_reduced, collision_model_reduced = (
geometric_models_reduced[0],
geometric_models_reduced[1],
)

# Check dimensions of the reduced model
# options 1-3 only take joint ids
print("joints to lock (only ids):", jointsToLockIDs)
print("reduced model: dim=" + str(len(model_reduced.joints)))
print("-" * 30)

# Option 4: Build a reduced model of a robot using RobotWrapper
# reference_configuration is optional: if not provided, neutral configuration used
# you can even mix joint names and joint ids
mixed_jointsToLockIDs = [jointsToLockIDs[0], "wrist_2_joint", "wrist_3_joint"]
robot = pin.RobotWrapper.BuildFromURDF(urdf_filename, mesh_dir)
reduced_robot = robot.buildReducedRobot(
list_of_joints_to_lock=mixed_jointsToLockIDs,
reference_configuration=initialJointConfig,
)

# Check dimensions of the reduced model and joint info
print("mixed joints to lock (names and ids):", mixed_jointsToLockIDs)
print("RobotWrapper reduced model: dim=" + str(len(reduced_robot.model.joints)))
for jn in robot.model.joints:
print(jn)

C++

#include "pinocchio/parsers/urdf.hpp"

#include "pinocchio/algorithm/joint-configuration.hpp"
#include "pinocchio/algorithm/model.hpp"

#include <iostream>
#include <algorithm>

// EXAMPLE_ROBOT_DATA_MODEL_DIR is defined by the CMake but you can define your own directory here.
#ifndef EXAMPLE_ROBOT_DATA_MODEL_DIR
#define EXAMPLE_ROBOT_DATA_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) ? EXAMPLE_ROBOT_DATA_MODEL_DIR + std::string("/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;
}