Commit f31382d8 authored by Carlos Mastalli's avatar Carlos Mastalli
Browse files

[loader] Added the free-flyer joint limits to all the floating-base robots

parent 21e463a2
......@@ -23,6 +23,17 @@ def readParamsFromSrdf(robot, SRDF_PATH, verbose):
return
def addFreeFlyerJointLimits(robot):
rmodel = robot.model
ub = rmodel.upperPositionLimit
ub[:7] = 1
rmodel.upperPositionLimit = ub
lb = rmodel.lowerPositionLimit
lb[:7] = -1
rmodel.lowerPositionLimit = lb
def loadTalosArm():
URDF_FILENAME = "talos_left_arm.urdf"
URDF_SUBPATH = "/talos_data/robots/" + URDF_FILENAME
......@@ -48,6 +59,8 @@ def loadTalos():
# Load SRDF file
readParamsFromSrdf(robot, modelPath + SRDF_SUBPATH, False)
assert ((robot.model.armature[:6] == 0.).all())
# Add the free-flyer joint limits
addFreeFlyerJointLimits(robot)
return robot
......@@ -61,6 +74,8 @@ def loadHyQ():
robot = RobotWrapper.BuildFromURDF(modelPath + URDF_SUBPATH, [modelPath], pinocchio.JointModelFreeFlyer())
# Load SRDF file
readParamsFromSrdf(robot, modelPath + SRDF_SUBPATH, False)
# Add the free-flyer joint limits
addFreeFlyerJointLimits(robot)
return robot
......@@ -103,4 +118,6 @@ def loadICub(reduced=True):
robot = RobotWrapper.BuildFromURDF(modelPath + URDF_SUBPATH, [modelPath], pinocchio.JointModelFreeFlyer())
# Load SRDF file
readParamsFromSrdf(robot, modelPath + SRDF_SUBPATH, False)
# Add the free-flyer joint limits
addFreeFlyerJointLimits(robot)
return robot
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