Unverified Commit 4c0d47ae authored by Justin Carpentier's avatar Justin Carpentier Committed by GitHub
Browse files

Merge pull request #1235 from julesser/devel

Add python example and doc for buildReducedModel
parents a3d3532c 485aec08
# 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
\include build-reduced-model.py
## C++
\include build-reduced-model.cpp
\ No newline at end of file
......@@ -71,6 +71,7 @@ IF(BUILD_PYTHON_INTERFACE)
LIST(APPEND ${PROJECT_NAME}_PYTHON_EXAMPLES
overview-urdf
gepetto-viewer
build-reduced-model
meshcat-viewer
meshcat-viewer-dae
robot-wrapper-viewer
......
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)
Markdown is supported
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment