class Talos(AbstractHumanoidRobot):
"""
This class defines a Talos robot
"""
forceSensorInLeftAnkle = ((1., 0., 0., 0.), (0., 1., 0., 0.),
(0., 0., 1., -0.107), (0., 0., 0., 1.))
forceSensorInRightAnkle = ((1., 0., 0., 0.), (0., 1., 0., 0.),
(0., 0., 1., -0.107), (0., 0., 0., 1.))
defaultFilename = "package://talos_data/urdf/talos_reduced_v2.urdf"
"""
TODO: Confirm the position and existence of these sensors
accelerometerPosition = np.matrix ((
(1., 0., 0., -.13,),
(0., 1., 0., 0.,),
(0., 0., 1., .118,),
(0., 0., 0., 1.,),
))
gyrometerPosition = np.matrix ((
(1., 0., 0., -.13,),
(0., 1., 0., 0.,),
(0., 0., 1., .118,),
(0., 0., 0., 1.,),
))
"""
def smallToFull(self, config):
# Gripper position in full configuration: 27:34, and 41:48
# Small configuration: 36 DOF
# Full configuration: 50 DOF
res = config[0:27] + 7 * (0., ) + config[27:34] \
+ 7 * (0., ) + config[34:]
return res
def __init__(self, name, device=None, tracer=None, fromRosParam=False):
self.OperationalPointsMap = {
'left-wrist': 'arm_left_7_joint',
'right-wrist': 'arm_right_7_joint',
'left-ankle': 'leg_left_6_joint',
'right-ankle': 'leg_right_6_joint',
'gaze': 'head_2_joint',
'waist': 'root_joint',
'chest': 'torso_2_joint'
}
if fromRosParam:
ltimeStep = 0.005
if device is not None:
ltimeStep = device.getTimeStep()
print("Using SoT parameter \"/robot_description\"")
paramName = "/robot_description"
self.param_server = ParameterServer("param_server")
self.param_server.init_simple(ltimeStep)
self.param_server.setParameter("/pg/remap/l_ankle",
"leg_left_6_link")
self.param_server.setParameter("/pg/remap/r_ankle",
"leg_right_6_link")
self.param_server.setParameter("/pg/remap/l_wrist",
"arm_left_7_link")
self.param_server.setParameter("/pg/remap/r_wrist",
"arm_right_7_link")
self.param_server.setParameter("/pg/remap/body", "base_link")
self.param_server.setParameter("/pg/remap/torso", "torso_2_link")
lpn_pre = "/robot/specificities/feet/"
feet = ['right', 'left']
for afoot in feet:
self.param_server.setParameterDbl(
lpn_pre + afoot + "/size/height", 0.122)
self.param_server.setParameterDbl(
lpn_pre + afoot + "/size/width", 0.205)
self.param_server.setParameterDbl(
lpn_pre + afoot + "/size/depth", 0.107)
self.param_server.setParameterDbl(
lpn_pre + afoot + "/anklePosition/x", 0.0)
self.param_server.setParameterDbl(
lpn_pre + afoot + "/anklePosition/y", 0.0)
self.param_server.setParameterDbl(
lpn_pre + afoot + "/anklePosition/z", 0.107)
self.param_server.displayRobotUtil()
model2_string = self.param_server.getParameter(paramName)
self.loadModelFromString(
model2_string,
rootJointType=pinocchio.JointModelFreeFlyer,
removeMimicJoints=True)
else:
self.loadModelFromUrdf(self.defaultFilename,
rootJointType=pinocchio.JointModelFreeFlyer,
removeMimicJoints=True)
assert hasattr(self, "pinocchioModel")
assert hasattr(self, "pinocchioData")
if device is not None:
self.device = device
AbstractHumanoidRobot.__init__(self, name, tracer)
self.OperationalPoints.append('waist')
self.OperationalPoints.append('chest')
# Create rigid body dynamics model and data (pinocchio)
self.dynamic = DynamicPinocchio(self.name + "_dynamic")
self.dynamic.setModel(self.pinocchioModel)
self.dynamic.setData(self.pinocchioData)
self.dynamic.displayModel()
self.dimension = self.dynamic.getDimension()
self.initializeRobot()
self.AdditionalFrames.append(
("leftFootForceSensor", self.forceSensorInLeftAnkle,
self.OperationalPointsMap["left-ankle"]))
self.AdditionalFrames.append(
("rightFootForceSensor", self.forceSensorInRightAnkle,
self.OperationalPointsMap["right-ankle"]))
# Create operational points based on operational points map
# (if provided)
if self.OperationalPointsMap is not None:
self.initializeOpPoints()
class Tiago(AbstractRobot):
"""
This class defines a Tiago robot
"""
defaultFilename = "package://tiago_data/robots/tiago_steel.urdf"
def defineHalfSitting(self, q):
"""
q is the configuration to fill.
When this function is called, the attribute pinocchioModel has been filled.
"""
model = self.pinocchioModel
# set arm position
self.setJointValueInConfig(q,
[ "arm_{}_joint".format(i+1) for i in range(7) ],
(0., -1.569796, -1.569796, 2.355194, 0., 0., 0.,))
def __init__(self, robotName, device=None, tracer=None, with_wheels=True, fromRosParam=False):
self.OperationalPointsMap = {
'wrist': 'arm_7_joint',
'right-wheel': 'wheel_right_joint',
'left-wheel': 'wheel_left_joint',
'mobilebase': 'root_joint',
'footprint': 'base_footprint_joint',
'gaze': 'head_2_joint',
}
if fromRosParam:
print("Using ROS parameter \"/robot_description\"")
rosParamName = "/robot_description"
import rospy
if rosParamName not in rospy.get_param_names():
raise RuntimeError('"' + rosParamName + '" is not a ROS parameter.')
s = rospy.get_param(rosParamName)
self.loadModelFromString(s, rootJointType=pinocchio.JointModelFreeFlyer,
removeMimicJoints=True)
else:
self.loadModelFromUrdf(self.defaultFilename,
rootJointType=pinocchio.JointModelFreeFlyer,
removeMimicJoints=True)
# Clean the robot model. Remove:
# - caster joints
# - suspension joints
# - for hey5 hands, remove every hand_* joints except hand_thumb_joint,
# hand_index_joint and hand_mrl_joint (mrl = middle, ring, little)
jointsToRemove = []
for name in self.pinocchioModel.names:
if not with_wheels and name.startswith("wheel_"):
jointsToRemove.append(name)
elif name.startswith('caster'):
jointsToRemove.append(name)
elif name.startswith('suspension'):
jointsToRemove.append(name)
elif name.startswith('hand_') and \
name not in ("hand_thumb_joint", "hand_index_joint", "hand_mrl_joint"):
jointsToRemove.append(name)
print("Removing joints " + ", ".join(jointsToRemove))
self.removeJoints(jointsToRemove)
assert hasattr(self, "pinocchioModel")
assert hasattr(self, "pinocchioData")
AbstractRobot.__init__(self, robotName, tracer)
# Create rigid body dynamics model and data (pinocchio)
self.dynamic = DynamicPinocchio(self.name + "_dynamic")
self.dynamic.setModel(self.pinocchioModel)
self.dynamic.setData(self.pinocchioData)
self.dynamic.displayModel()
self.dimension = self.dynamic.getDimension()
self.device = device
self.initializeRobot()
# Create operational points based on operational points map (if provided)
if self.OperationalPointsMap is not None:
self.initializeOpPoints()
def setClosedLoop(self, closedLoop):
if closedLoop:
plug(self.device.robotState, self.dynamic.position)
self.device.setClosedLoop(True)
else:
plug(self.device.state, self.dynamic.position)
self.device.setClosedLoop(False)
def _initialize(self):
AbstractRobot._initialize(self)
self.OperationalPoints.extend(['wrist', 'left-wheel', 'right-wheel', 'footprint', 'mobilebase', 'gaze'])
class Talos(AbstractHumanoidRobot):
"""
This class defines a Talos robot
"""
forceSensorInLeftAnkle = ((1., 0., 0., 0.), (0., 1., 0., 0.),
(0., 0., 1., -0.107), (0., 0., 0., 1.))
forceSensorInRightAnkle = ((1., 0., 0., 0.), (0., 1., 0., 0.),
(0., 0., 1., -0.107), (0., 0., 0., 1.))
defaultFilename = "package://talos_data/urdf/talos_reduced_v2.urdf"
"""
TODO: Confirm the position and existence of these sensors
accelerometerPosition = np.matrix ((
(1., 0., 0., -.13,),
(0., 1., 0., 0.,),
(0., 0., 1., .118,),
(0., 0., 0., 1.,),
))
gyrometerPosition = np.matrix ((
(1., 0., 0., -.13,),
(0., 1., 0., 0.,),
(0., 0., 1., .118,),
(0., 0., 0., 1.,),
))
"""
def smallToFull(self, config):
# Gripper position in full configuration: 27:34, and 41:48
# Small configuration: 36 DOF
# Full configuration: 50 DOF
res = config[0:27] + 7 * (0., ) + config[27:34] + 7 * (
0., ) + config[34:]
return res
def __init__(self,
name,
initialConfig,
device=None,
tracer=None,
fromRosParam=False):
self.OperationalPointsMap = {
'left-wrist': 'arm_left_7_joint',
'right-wrist': 'arm_right_7_joint',
'left-ankle': 'leg_left_6_joint',
'right-ankle': 'leg_right_6_joint',
'gaze': 'head_2_joint',
'waist': 'root_joint',
'chest': 'torso_2_joint'
}
if fromRosParam:
print("Using ROS parameter \"/robot_description\"")
rosParamName = "/robot_description"
import rospy
if rosParamName not in rospy.get_param_names():
raise RuntimeError('"' + rosParamName +
'" is not a ROS parameter.')
s = rospy.get_param(rosParamName)
self.loadModelFromString(
s,
rootJointType=pinocchio.JointModelFreeFlyer,
removeMimicJoints=True)
else:
self.loadModelFromUrdf(self.defaultFilename,
rootJointType=pinocchio.JointModelFreeFlyer,
removeMimicJoints=True)
assert hasattr(self, "pinocchioModel")
assert hasattr(self, "pinocchioData")
AbstractHumanoidRobot.__init__(self, name, tracer)
self.OperationalPoints.append('waist')
self.OperationalPoints.append('chest')
# Create rigid body dynamics model and data (pinocchio)
self.dynamic = DynamicPinocchio(self.name + "_dynamic")
self.dynamic.setModel(self.pinocchioModel)
self.dynamic.setData(self.pinocchioData)
self.dynamic.displayModel()
self.dimension = self.dynamic.getDimension()
self.device = device
self.initializeRobot()
self.AdditionalFrames.append(
("leftFootForceSensor", self.forceSensorInLeftAnkle,
self.OperationalPointsMap["left-ankle"]))
self.AdditionalFrames.append(
("rightFootForceSensor", self.forceSensorInRightAnkle,
self.OperationalPointsMap["right-ankle"]))
# Create operational points based on operational points map (if provided)
if self.OperationalPointsMap is not None:
self.initializeOpPoints()