class Pr2(AbstractHumanoidRobot):
"""
This class instanciates a Pr2 robot.
"""
OperationalPoints = [
'right-wrist', 'left-wrist', 'waist', 'gaze', 'chest', 'left-ankle'
]
jointMap = {}
jointMap['BODY'] = 'base_link'
jointMap['l_wrist'] = 'l_gripper_palm_link'
jointMap['r_wrist'] = 'r_gripper_palm_link'
jointMap['l_gripper'] = 'l_gripper_tool_frame'
jointMap['r_gripper'] = 'r_gripper_tool_frame'
jointMap['gaze'] = 'double_stereo_link'
jointMap['torso'] = 'torso_lift_link'
jointMap['l_ankle'] = 'base_link' # TODO?
tracedSignals = {
'dynamic': ["com", "position", "velocity", "acceleration"],
'device': ['control', 'state']
}
def specifySpecialLinks(self):
for i in self.jointMap:
self.dynamic.addJointMapping(i, self.jointMap[i])
def __init__(self, name, device=None, tracer=None):
AbstractHumanoidRobot.__init__(self, name, tracer)
self.device = device
self.dynamic = RosRobotModel("{0}_dynamic".format(name))
self.specifySpecialLinks()
#Note: the param 'robot_description' should be defined before the next instruction
self.dynamic.loadFromParameterServer()
self.dimension = self.dynamic.getDimension()
self.halfSitting = (0., ) * self.dimension
if self.device:
self.device.control.value = self.dimension * (0., )
lst = list(self.halfSitting)
lst[24] = -0.33
lst[26] = -0.47
lst[39] = -0.33
lst[41] = -0.47
self.halfSitting = tuple(lst)
# correct the initialization of the dynamic.
self.dynamic.velocity.value = self.dimension * (0., )
self.dynamic.acceleration.value = self.dimension * (0., )
self.dynamic.ffposition.unplug()
self.dynamic.ffvelocity.unplug()
self.dynamic.ffacceleration.unplug()
self.dynamic.setProperty('ComputeBackwardDynamics', 'true')
self.dynamic.setProperty('ComputeAccelerationCoM', 'true')
self.initializeRobot()
class Pr2(AbstractHumanoidRobot):
"""
This class instanciates a Pr2 robot.
"""
OperationalPoints = ['right-wrist','left-wrist','waist','gaze','chest','left-ankle']
jointMap={}
jointMap['BODY'] = 'base_link'
jointMap['l_wrist'] = 'l_gripper_palm_link'
jointMap['r_wrist'] = 'r_gripper_palm_link'
jointMap['l_gripper'] = 'l_gripper_tool_frame'
jointMap['r_gripper'] = 'r_gripper_tool_frame'
jointMap['gaze'] = 'double_stereo_link'
jointMap['torso'] = 'torso_lift_link'
jointMap['l_ankle'] = 'base_link' # TODO?
tracedSignals = {
'dynamic': ["com", "position", "velocity", "acceleration"],
'device': ['control', 'state']
}
def specifySpecialLinks(self):
for i in self.jointMap:
self.dynamic.addJointMapping(i, self.jointMap[i])
def __init__(self, name, device = None, tracer = None):
AbstractHumanoidRobot.__init__ (self, name, tracer)
self.device = device
self.dynamic = RosRobotModel("{0}_dynamic".format(name))
self.specifySpecialLinks()
#Note: the param 'robot_description' should be defined before the next instruction
self.dynamic.loadFromParameterServer()
self.dimension = self.dynamic.getDimension()
self.halfSitting = (0.,) * self.dimension
if self.device:
self.device.control.value = self.dimension * (0.,)
lst = list(self.halfSitting)
lst[24] = -0.33
lst[26] = -0.47
lst[39] = -0.33
lst[41] = -0.47
self.halfSitting = tuple(lst)
# correct the initialization of the dynamic.
self.dynamic.velocity.value = self.dimension*(0.,)
self.dynamic.acceleration.value = self.dimension*(0.,)
self.dynamic.ffposition.unplug()
self.dynamic.ffvelocity.unplug()
self.dynamic.ffacceleration.unplug()
self.dynamic.setProperty('ComputeBackwardDynamics','true')
self.dynamic.setProperty('ComputeAccelerationCoM','true')
self.initializeRobot()
