def loadSoloLeg(initViewer=False):
# Load Solo8.
URDF_FILENAME = "solo.urdf"
SRDF_FILENAME = "solo.srdf"
SRDF_SUBPATH = "/solo_description/srdf/" + SRDF_FILENAME
URDF_SUBPATH = "/solo_description/robots/" + URDF_FILENAME
modelPath = getModelPath(URDF_SUBPATH)
robot = pio.RobotWrapper.BuildFromURDF(modelPath + URDF_SUBPATH,
[modelPath])
readParamsFromSrdf(robot, modelPath + SRDF_SUBPATH, False, False,
"standing")
# Create Solo-leg model from Solo8
lock = list(range(3, 9))
rmodel = pio.buildReducedModel(robot.model, lock, robot.q0)
rdata = rmodel.createData()
# Create solo-leg geometry model (visuals and collisions
reduceGeomModel(robot.model, robot.data, rmodel, rdata, robot.visual_model,
robot.visual_data, robot.q0, robot.q0[:2])
reduceGeomModel(robot.model, robot.data, rmodel, rdata,
robot.collision_model, robot.collision_data)
# Create Solo robot-wrapper
rw = pio.RobotWrapper(rmodel,
collision_model=robot.collision_model,
visual_model=robot.visual_model)
if initViewer:
rw.initViewer(loadModel=True)
for g in rw.visual_model.geometryObjects:
if g.parentJoint == 0:
rw.viewer.gui.setVisibility(
'world/pinocchio/visuals/' + g.name, 'OFF')
return rw
def reduceModel(self, jointsToRemove, config, len_prefix=0):
jointIds = [
self.model.getJointId(jn[len_prefix:]) for jn in jointsToRemove
]
self.model, self.gmodel = pinocchio.buildReducedModel(
self.model, self.gmodel, jointIds, np.array(config))
self.data = pinocchio.Data(self.model)
self.gdata = pinocchio.GeometryData(self.gmodel)
def removeJoints(self, joints):
"""
- param joints: a list of joint names to be removed from the self.pinocchioModel
"""
jointIds = list()
for j in joints:
if self.pinocchioModel.existJointName(j):
jointIds.append(self.pinocchioModel.getJointId(j))
if len(jointIds) > 0:
q = pinocchio.neutral(self.pinocchioModel)
self.pinocchioModel = pinocchio.buildReducedModel(self.pinocchioModel, jointIds, q)
self.pinocchioData = pinocchio.Data(self.pinocchioModel)
# 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)
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]
import numpy as np
import pinocchio as pin
import example_robot_data as robex
from scipy.optimize import fmin_bfgs
from numpy.linalg import inv, pinv, eig, norm, svd, det
import time
pin.seed(int(time.time())) # change me :)
np.set_printoptions(precision=4, linewidth=200, suppress=True)
# --- Load robot model
robot = robex.loadTiagoNoHand()
robot.model = pin.buildReducedModel(robot.model, [11, 12], robot.q0)
robot.q0 = robot.q0[:-4]
robot.rebuildData()
#robot = robex.loadSolo()
#robot = robex.loadUR()
robot = robex.loadTalos()
robot.initViewer(loadModel=True)
robot.display(robot.q0)
NQ = robot.model.nq
NV = robot.model.nv
#robot.q0[:] = [0, -3.14/3, -3.14/2, 0, 0, 0]
# --- Add box to represent target
gv = robot.viewer.gui
gv.addBox("world/box", .05, .1, .2, [1., .2, .2, .5])
gv.addSphere("world/ball", .05, [.2, .2, 1., .5])
gv.applyConfiguration('world/box', [0.5, .2, .2, 1, 0, 0, 0])
