Build Reduced Model: How to Fix Joints at Specified Values?

[julesser]

Hi all,

I was wondering whether it's possible to load a full urdf model and then selectively fix some joints at some specified values? Does the buildReducedModel API provide this functionality and is it accessible via the python bindings?

Right now I am working with Crocoddyl and utilize the robot wrapper that comes along with Pinnochio in order to build a floating base model of a humanoid robot like this:

rh5_robot = RobotWrapper.BuildFromURDF(modelPath + URDF_SUBPATH, [modelPath], pinocchio.JointModelFreeFlyer())
rmodel = rh5_robot.model
addFreeFlyerJointLimits(rmodel)

[jcarpent]

Hi @julesser,

From the Python documentation of Pinocchio:

pin.buildReducedModel?                                                                                                                                                                                                                                                                                                                                              
Docstring:
buildReducedModel( (Model)model, (StdVec_Index)list_of_joints_to_lock, (object)reference_configuration) -> Model :
    Build a reduce model from a given input model and a list of joint to lock.

    Parameters:
        model: input kinematic modell to reduce
        list_of_joints_to_lock: list of joint indexes to lock
        reference_configuration: reference configuration to compute the placement of the lock joints

buildReducedModel( (Model)model, (GeometryModel)geom_model, (StdVec_Index)list_of_joints_to_lock, (object)reference_configuration) -> tuple :
    Build a reduced model and a rededuced geometry model  from a given input model,a given input geometry model and a list of joint to lock.

    Parameters:
        model: input kinematic modell to reduce
        geom_model: input geometry model to reduce
        list_of_joints_to_lock: list of joint indexes to lock
        reference_configuration: reference configuration to compute the placement of the lock joints

I've just realized the mistakes in the doc, but should be clear enough to help you. Best,

Justin

[jcarpent]

With no activity, I will close this questionning issue. Feel free to reopen it if needed.

[julesser]

@jcarpent , thanks for your kind reply. I've adapted the build procedure from the build-reduced-model.cpp example code as follows:

import pinocchio
# Load the full model 
rh5_robot = RobotWrapper.BuildFromURDF(modelPath + URDF_SUBPATH, [modelPath], pinocchio.JointModelFreeFlyer()) # Load URDF file
rmodel = rh5_robot.model
# Create a list of joints to lock
jointsToLock = ['ALShoulder1', 'ALShoulder2', 'ALShoulder3', 'ALElbow', 'ALWristRoll', 'ALWristYaw', 'ALWristPitch',
                'ARShoulder1', 'ARShoulder2', 'ARShoulder3', 'ARElbow', 'ARWristRoll', 'ARWristYaw', 'ARWristPitch',
                'HeadPitch', 'HeadRoll', 'HeadYaw']
# Get the joint IDs
jointsToLockIDs = []
for jn in range(len(jointsToLock)):
    jointsToLockIDs.append(rmodel.getFrameId(jointsToLock[jn]))
# Set initial configuration
fixedJointConfig = [-0.5, 0.5, -0.3, 0, 0, 0,  
                    0.5, -0.5, 0.3, 0, 0, 0,
                    0, 0, 0]
# Build the reduced model
reducedModel = buildReducedModel(rmodel, jointsToLockIDs, fixedJointConfig)
# Add joint limits
addFreeFlyerJointLimits(rmodel)

Two questions:

1. How to correctly specify the import? Right now I get NameError: name 'buildReducedModel' is not defined I've installedrobotpkg-py36-pinocchio according to the description here and setup environment variables as

   export PATH=/opt/openrobots/bin:$PATH
   export PKG_CONFIG_PATH=/opt/openrobots/lib/pkgconfig:$PKG_CONFIG_PATH
   export LD_LIBRARY_PATH=/opt/openrobots/lib:$LD_LIBRARY_PATH
   export PYTHONPATH=/opt/openrobots/lib/python3.6/site-packages:$PYTHONPATH

2. Where can I find the mentioned python documentation?

[julesser]

With no activity, I will close this questionning issue. Feel free to reopen it if needed.

@jcarpent I don't have permission to reopen the issue.

[jcarpent]

buildReducedModel is inside Pinocchio namspace. So:

import pinocchio as pin
pin.buildReducedModel(...)

[julesser]

Obviously correct. Thanks for pointing me to this!

[julesser]

In case someone else wants to use the buildReducedModel API via the Python bindings, here is a working python code snipped, based on the C++ example, for building a reduced model + geometric model. Note that also building the geometric model is necessary only if you want to display your robot in the gepetto-viewer.

# Load the full model 
robot = RobotWrapper.BuildFromURDF(modelPath + URDF_SUBPATH, [modelPath], pinocchio.JointModelFreeFlyer()) # Load URDF file
# print('standard model: dim=' + str(len(robot.model.joints)))
# Create a list of joints to lock
jointsToLock = ['ALShoulder1', 'ALShoulder2', 'ALShoulder3', 'ALElbow', 'ALWristRoll', 'ALWristYaw', 'ALWristPitch',
                'ARShoulder1', 'ARShoulder2', 'ARShoulder3', 'ARElbow', 'ARWristRoll', 'ARWristYaw', 'ARWristPitch',
                'HeadPitch', 'HeadRoll', 'HeadYaw']
# Get the existing joint IDs
jointsToLockIDs = []
for jn in range(len(jointsToLock)):
    if robot.model.existJointName(jointsToLock[jn]):
        jointsToLockIDs.append(robot.model.getJointId(jointsToLock[jn]))
    else:
        print('Warning: joint ' + str(jointsToLock[jn]) + ' does not belong to the model!')
# Set initial position of fixed joints; Your remaining revolute joints could be initialized also here or later. 
initialJointConfig = np.matrix([0,0,0,0,0,0,0, # Floating Base
                         0,0,0,                 # Torso
                         -0.5,0.5,0,-0.3,0,0,0, # Left Arm
                         0.5,-0.5,0,0.3,0,0,0,  # Right Arm
                         0,0,0,                 # Head
                         0,0,0,0,0,0,           # Left Leg     
                         0,0,0,0,0,0]).T        # Right Leg)
# Build the reduced model (including the geometric model for proper displaying of the robot)
robot.model, robot.visual_model = pinocchio.buildReducedModel(robot.model, robot.visual_model, jointsToLockIDs, initialJointConfig)
# In case no display needed:
# robot.model = pinocchio.buildReducedModel(robot.model, jointsToLockIDs, initialJointConfig)
# print('reduced model: dim=' + str(len(robot.model.joints)))

[olivier-stasse]

Dear @julesser this is a welcome contribution, if @jcarpent is fine it would be nice if you could propose a PR on the documentation to have it in the repository. It would be easier than going through the issues. All the best.

[jcarpent]

Yes, I do agree with @olivier-stasse, it would be nice if @julesser you can provide this tiny example along with Pinocchio. It will be very helpful for orther users. Thanks in advance !