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