class Hrp2(AbstractHumanoidRobot):
"""
This class instantiates a Hrp2 robot
"""
forceSensorInLeftAnkle = ((1., 0., 0., 0.), (0., 1., 0., 0.),
(0., 0., 1., -0.105), (0., 0., 0., 1.))
forceSensorInRightAnkle = ((1., 0., 0., 0.), (0., 1., 0., 0.),
(0., 0., 1., -0.105), (0., 0., 0., 1.))
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):
res = (config + 10 * (0., ))
return res
def __init__(self, name, robotnumber, device=None, tracer=None):
AbstractHumanoidRobot.__init__(self, name, tracer)
self.OperationalPoints.append('waist')
self.OperationalPoints.append('chest')
self.device = device
self.AdditionalFrames.append(
("accelerometer", matrixToTuple(self.accelerometerPosition),
"chest"))
self.AdditionalFrames.append(
("gyrometer", matrixToTuple(self.gyrometerPosition), "chest"))
self.AdditionalFrames.append(
("leftFootForceSensor", self.forceSensorInLeftAnkle, "left-ankle"))
self.AdditionalFrames.append(
("rightFootForceSensor", self.forceSensorInRightAnkle,
"right-ankle"))
self.OperationalPointsMap = {
'left-wrist': 'LARM_JOINT5',
'right-wrist': 'RARM_JOINT5',
'left-ankle': 'LLEG_JOINT5',
'right-ankle': 'RLEG_JOINT5',
'gaze': 'HEAD_JOINT1',
'waist': 'WAIST',
'chest': 'CHEST_JOINT1'
}
self.dynamic = RosRobotModel("{0}_dynamic".format(name))
rospack = RosPack()
self.urdfPath = rospack.get_path('hrp2_{0}_description'.format(
robotnumber)) + '/urdf/hrp2_{0}_reduced.urdf'.format(robotnumber)
self.pinocchioModel = se3.buildModelFromUrdf(self.urdfPath,
se3.JointModelFreeFlyer())
self.pinocchioData = self.pinocchioModel.createData()
self.dynamic.setModel(self.pinocchioModel)
self.dynamic.setData(self.pinocchioData)
self.dimension = self.dynamic.getDimension()
self.plugVelocityFromDevice = True
if self.dimension != len(self.halfSitting):
raise RuntimeError(
"Dimension of half-sitting: {0} differs from dimension of robot: {1}"
.format(len(self.halfSitting), self.dimension))
self.initializeRobot()
self.dynamic.displayModel()
class Robot(AbstractHumanoidRobot):
"""
This class instanciates Aldebaran Romeo robot
"""
# half sitting position for the given romeo_small urdf model
# coordinates should be given in depth
# moreover, we do not consider fixed joint
halfSittingSmall = (
# Free flyer
0,
0,
0.840252,
0,
0,
0,
# ImuTorsoAccelerometer_joint
#0.0,
# ImuTorsoGyrometer_joint
#0.0,
# LHipYaw, LHipRoll, LHipPtch, LKneePitch, LAnklePitch, L_AnkleRoll
0.0,
0.0,
-0.3490658,
0.6981317,
-0.3490658,
0.0,
# LFsrCenterJoint, LFsrFL_joint, LFsrFR_joint, LFsrRCenter_joint,
#0.0, 0.0, 0.0, 0.0,
# l_sole_joint
#0.0,
# RHipYaw, RHipRoll, RHipPtch, RKneePitch, RAnklePitch, R_AnkleRoll
0.0,
0.0,
-0.3490658,
0.6981317,
-0.3490658,
0.0,
# RFsrCenterJoint, RFsrFL_joint, RFsrFR_joint, RFsrRCenter_joint,
#0.0, 0.0, 0.0, 0.0,
# r_sole_joint
#0.0,
# TrunkYaw
0.0,
# LShoulderPitch, LShoulderYaw, LElbowRoll, LElbowYaw,
1.5,
0.6,
-0.5,
-1.05,
# LWristRoll, LWristYaw, LWristPitch,
-0.4,
-0.3,
-0.2,
# LFinger21, LFinger22, LFinger23
#0.0, 0.0, 0.0,
# LFinger31, LFinger32, LFinger33,
#0.0, 0.0, 0.0,
# LHand,
#0.0,
# LFinger12, LFinger13
#0.0, 0.0,
# LThumb1, LThumb2, LThumb3
#0.0, 0.0, 0.0,
# l_gripper_joint
#0.0,
# NeckYaw, NeckPitch,
0.0,
0.0,
# HeadPitch, HeadRoll
0.0,
0.0,
# CameraDepth_joint,
#0.0,
# CameraLeftEye_joint, CameraLeft_joint,
#0.0, 0.0,
# CameraRightEye_joint, CameraRight_joint,
#0.0, 0.0,
# HeadTouchFront_joint, HeadTouchMiddle_joint
#0.0, 0.0,
# ImuHeadAccelerometer_joint, ImuHeadGyrometer_joint
#0.0, 0.0,
# gaze_joint
#0.0,
# RShoulderPitch, RShoulderYaw, RElbowRoll, RElbowYaw,
1.5,
-0.6,
0.5,
1.05,
# RWristRoll, RWristYaw, RWristPitch,
-0.4,
-0.3,
-0.2,
# RFinger21, RFinger22, RFinger23
#0.0, 0.0, 0.0,
# RFinger31, RFinger32, RFinger33,
#0.0, 0.0, 0.0,
# RHand,
#0.0,
# RFinger12, RFinger13
# 0.0, 0.0,
# RThumb1, RThumb2, RThumb3
#0.0, 0.0, 0.0,
# r_gripper_joint
#0.0,
)
# half sitting position for the romeo urdf model
# coordinates should be given in depth
# moreover, we do not consider fixed joint
halfSittingAll = (
# Free flyer
0,
0,
0.840252,
0,
0,
0,
# ImuTorsoAccelerometer_joint
#0.0,
# ImuTorsoGyrometer_joint
#0.0,
# LHipYaw, LHipRoll, LHipPtch, LKneePitch, LAnklePitch, L_AnkleRoll
0.0,
0.0,
-0.3490658,
0.6981317,
-0.3490658,
0.0,
# LFsrCenterJoint, LFsrFL_joint, LFsrFR_joint, LFsrRCenter_joint,
#0.0, 0.0, 0.0, 0.0,
# l_sole_joint
#0.0,
# RHipYaw, RHipRoll, RHipPtch, RKneePitch, RAnklePitch, R_AnkleRoll
0.0,
0.0,
-0.3490658,
0.6981317,
-0.3490658,
0.0,
# RFsrCenterJoint, RFsrFL_joint, RFsrFR_joint, RFsrRCenter_joint,
#0.0, 0.0, 0.0, 0.0,
# r_sole_joint
#0.0,
# TrunkYaw
0.0,
# LShoulderPitch, LShoulderYaw, LElbowRoll, LElbowYaw,
1.5,
0.6,
-0.5,
-1.05,
# LWristRoll, LWristYaw, LWristPitch,
-0.4,
-0.3,
-0.2,
# LFinger21, LFinger22, LFinger23
0.0,
0.0,
0.0,
# LFinger31, LFinger32, LFinger33,
0.0,
0.0,
0.0,
# LHand, LFinger12, LFinger13
0.0,
0.0,
0.0,
# LThumb1, LThumb2, LThumb3
0.0,
0.0,
0.0,
# l_gripper_joint
#0.0,
# NeckYaw, NeckPitch,
0.0,
0.0,
# HeadPitch, HeadRoll
0.0,
0.0,
# CameraDepth_joint,
#0.0,
# CameraLeftEye_joint, CameraLeft_joint,
#0.0, 0.0,
# CameraRightEye_joint, CameraRight_joint,
#0.0, 0.0,
# HeadTouchFront_joint, HeadTouchMiddle_joint
#0.0, 0.0,
# ImuHeadAccelerometer_joint, ImuHeadGyrometer_joint
#0.0, 0.0,
# gaze_joint
#0.0,
# RShoulderPitch, RShoulderYaw, RElbowRoll, RElbowYaw,
1.5,
-0.6,
0.5,
1.05,
# RWristRoll, RWristYaw, RWristPitch,
-0.4,
-0.3,
-0.2,
# RFinger21, RFinger22, RFinger23
0.0,
0.0,
0.0,
# RFinger31, RFinger32, RFinger33,
0.0,
0.0,
0.0,
# RHead, RFinger12, RFinger13
0.0,
0.0,
0.0,
# RThumb1, RThumb2, RThumb3
0.0,
0.0,
0.0,
# r_gripper_joint
#0.0,
)
def __init__(self, name, device=None, tracer=None):
AbstractHumanoidRobot.__init__(self, name, tracer)
print "You ask for ", name, ". "
rospack = RosPack()
self.urdfDir = rospack.get_path('romeo_description') + '/urdf/'
if name == 'romeo_small':
print "Loaded model is romeo_small.urdf."
self.urdfName = 'romeo_small.urdf'
self.halfSitting = self.halfSittingSmall
else:
print "Loaded model is romeo.urdf."
self.urdfName = 'romeo.urdf'
self.halfSitting = self.halfSittingAll
self.OperationalPoints.append('waist')
self.OperationalPoints.append('chest')
self.OperationalPointsMap = {
'left-wrist': 'LWristPitch',
'right-wrist': 'RWristPitch',
'left-ankle': 'LAnkleRoll',
'right-ankle': 'RAnkleRoll',
'gaze': 'gaze_joint',
'waist': 'waist',
'chest': 'TrunkYaw'
}
self.device = device
# correct the name of the body link
self.dynamic = RosRobotModel("{0}_dynamic".format(name))
self.pinocchioModel = se3.buildModelFromUrdf(
self.urdfDir + self.urdfName, se3.JointModelFreeFlyer())
self.pinocchioData = self.pinocchioModel.createData()
self.dynamic.setModel(self.pinocchioModel)
self.dynamic.setData(self.pinocchioData)
# complete feet position (TODO: move it into srdf file)
#ankle =self.dynamic.getAnklePositionInFootFrame()
#self.ankleLength = 0.1935
#self.ankleWidth = 0.121
#self.dynamic.setFootParameters(True , self.ankleLength, self.ankleWidth, ankle)
#self.dynamic.setFootParameters(False, self.ankleLength, self.ankleWidth, ankle)
# check half sitting size
self.dimension = self.dynamic.getDimension()
if self.dimension != len(self.halfSitting):
raise RuntimeError(
"invalid half-sitting pose. HalfSitting Dimension:",
len(self.halfSitting), " .Robot Dimension:", self.dimension)
self.initializeRobot()
class Hrp2(AbstractHumanoidRobot):
"""
This class instantiates a Hrp2 robot
"""
forceSensorInLeftAnkle = ((1.,0.,0.,0.),
(0.,1.,0.,0.),
(0.,0.,1.,-0.105),
(0.,0.,0.,1.))
forceSensorInRightAnkle = ((1.,0.,0.,0.),
(0.,1.,0.,0.),
(0.,0.,1.,-0.105),
(0.,0.,0.,1.))
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):
res = (config + 10*(0.,))
return res
def __init__(self, name, robotnumber,
device = None, tracer = None):
AbstractHumanoidRobot.__init__ (self, name, tracer)
self.OperationalPoints.append('waist')
self.OperationalPoints.append('chest')
self.device = device
self.AdditionalFrames.append(
("accelerometer",
matrixToTuple(self.accelerometerPosition), "chest"))
self.AdditionalFrames.append(
("gyrometer",
matrixToTuple(self.gyrometerPosition), "chest"))
self.AdditionalFrames.append(
("leftFootForceSensor",
self.forceSensorInLeftAnkle, "left-ankle"))
self.AdditionalFrames.append(
("rightFootForceSensor",
self.forceSensorInRightAnkle, "right-ankle"))
self.OperationalPointsMap = {'left-wrist' : 'LARM_JOINT5',
'right-wrist' : 'RARM_JOINT5',
'left-ankle' : 'LLEG_JOINT5',
'right-ankle' : 'RLEG_JOINT5',
'gaze' : 'HEAD_JOINT1',
'waist' : 'WAIST',
'chest' : 'CHEST_JOINT1'}
self.dynamic = RosRobotModel("{0}_dynamic".format(name))
rospack = RosPack()
self.urdfPath = rospack.get_path('hrp2_{0}_description'.format(robotnumber)) + '/urdf/hrp2_{0}.urdf'.format(robotnumber)
self.pinocchioModel = se3.buildModelFromUrdf(self.urdfPath, se3.JointModelFreeFlyer())
self.pinocchioData = self.pinocchioModel.createData()
self.dynamic.setModel(self.pinocchioModel)
self.dynamic.setData(self.pinocchioData)
self.dimension = self.dynamic.getDimension()
self.plugVelocityFromDevice = True
if self.dimension != len(self.halfSitting):
raise RuntimeError("Dimension of half-sitting: {0} differs from dimension of robot: {1}".format (len(self.halfSitting), self.dimension))
self.initializeRobot()
self.dynamic.displayModel()
class Robot (AbstractHumanoidRobot):
"""
This class instanciates Aldebaran Romeo robot
"""
# half sitting position for the given romeo_small urdf model
# coordinates should be given in depth
# moreover, we do not consider fixed joint
halfSittingSmall = (
# Free flyer
0, 0, 0.840252, 0, 0, 0,
# ImuTorsoAccelerometer_joint
#0.0,
# ImuTorsoGyrometer_joint
#0.0,
# LHipYaw, LHipRoll, LHipPtch, LKneePitch, LAnklePitch, L_AnkleRoll
0.0, 0.0, -0.3490658, 0.6981317, -0.3490658, 0.0,
# LFsrCenterJoint, LFsrFL_joint, LFsrFR_joint, LFsrRCenter_joint,
#0.0, 0.0, 0.0, 0.0,
# l_sole_joint
#0.0,
# RHipYaw, RHipRoll, RHipPtch, RKneePitch, RAnklePitch, R_AnkleRoll
0.0, 0.0, -0.3490658, 0.6981317, -0.3490658, 0.0,
# RFsrCenterJoint, RFsrFL_joint, RFsrFR_joint, RFsrRCenter_joint,
#0.0, 0.0, 0.0, 0.0,
# r_sole_joint
#0.0,
# TrunkYaw
0.0,
# LShoulderPitch, LShoulderYaw, LElbowRoll, LElbowYaw,
1.5, 0.6, -0.5, -1.05,
# LWristRoll, LWristYaw, LWristPitch,
-0.4, -0.3, -0.2,
# LFinger21, LFinger22, LFinger23
#0.0, 0.0, 0.0,
# LFinger31, LFinger32, LFinger33,
#0.0, 0.0, 0.0,
# LHand,
#0.0,
# LFinger12, LFinger13
#0.0, 0.0,
# LThumb1, LThumb2, LThumb3
#0.0, 0.0, 0.0,
# l_gripper_joint
#0.0,
# NeckYaw, NeckPitch,
0.0, 0.0,
# HeadPitch, HeadRoll
0.0, 0.0,
# CameraDepth_joint,
#0.0,
# CameraLeftEye_joint, CameraLeft_joint,
#0.0, 0.0,
# CameraRightEye_joint, CameraRight_joint,
#0.0, 0.0,
# HeadTouchFront_joint, HeadTouchMiddle_joint
#0.0, 0.0,
# ImuHeadAccelerometer_joint, ImuHeadGyrometer_joint
#0.0, 0.0,
# gaze_joint
#0.0,
# RShoulderPitch, RShoulderYaw, RElbowRoll, RElbowYaw,
1.5, -0.6, 0.5, 1.05,
# RWristRoll, RWristYaw, RWristPitch,
-0.4, -0.3, -0.2,
# RFinger21, RFinger22, RFinger23
#0.0, 0.0, 0.0,
# RFinger31, RFinger32, RFinger33,
#0.0, 0.0, 0.0,
# RHand,
#0.0,
# RFinger12, RFinger13
# 0.0, 0.0,
# RThumb1, RThumb2, RThumb3
#0.0, 0.0, 0.0,
# r_gripper_joint
#0.0,
)
# half sitting position for the romeo urdf model
# coordinates should be given in depth
# moreover, we do not consider fixed joint
halfSittingAll = (
# Free flyer
0, 0, 0.840252, 0, 0, 0,
# ImuTorsoAccelerometer_joint
#0.0,
# ImuTorsoGyrometer_joint
#0.0,
# LHipYaw, LHipRoll, LHipPtch, LKneePitch, LAnklePitch, L_AnkleRoll
0.0, 0.0, -0.3490658, 0.6981317, -0.3490658, 0.0,
# LFsrCenterJoint, LFsrFL_joint, LFsrFR_joint, LFsrRCenter_joint,
#0.0, 0.0, 0.0, 0.0,
# l_sole_joint
#0.0,
# RHipYaw, RHipRoll, RHipPtch, RKneePitch, RAnklePitch, R_AnkleRoll
0.0, 0.0, -0.3490658, 0.6981317, -0.3490658, 0.0,
# RFsrCenterJoint, RFsrFL_joint, RFsrFR_joint, RFsrRCenter_joint,
#0.0, 0.0, 0.0, 0.0,
# r_sole_joint
#0.0,
# TrunkYaw
0.0,
# LShoulderPitch, LShoulderYaw, LElbowRoll, LElbowYaw,
1.5, 0.6, -0.5, -1.05,
# LWristRoll, LWristYaw, LWristPitch,
-0.4, -0.3, -0.2,
# LFinger21, LFinger22, LFinger23
0.0, 0.0, 0.0,
# LFinger31, LFinger32, LFinger33,
0.0, 0.0, 0.0,
# LHand, LFinger12, LFinger13
0.0, 0.0, 0.0,
# LThumb1, LThumb2, LThumb3
0.0, 0.0, 0.0,
# l_gripper_joint
#0.0,
# NeckYaw, NeckPitch,
0.0, 0.0,
# HeadPitch, HeadRoll
0.0, 0.0,
# CameraDepth_joint,
#0.0,
# CameraLeftEye_joint, CameraLeft_joint,
#0.0, 0.0,
# CameraRightEye_joint, CameraRight_joint,
#0.0, 0.0,
# HeadTouchFront_joint, HeadTouchMiddle_joint
#0.0, 0.0,
# ImuHeadAccelerometer_joint, ImuHeadGyrometer_joint
#0.0, 0.0,
# gaze_joint
#0.0,
# RShoulderPitch, RShoulderYaw, RElbowRoll, RElbowYaw,
1.5, -0.6, 0.5, 1.05,
# RWristRoll, RWristYaw, RWristPitch,
-0.4, -0.3, -0.2,
# RFinger21, RFinger22, RFinger23
0.0, 0.0, 0.0,
# RFinger31, RFinger32, RFinger33,
0.0, 0.0, 0.0,
# RHead, RFinger12, RFinger13
0.0, 0.0, 0.0,
# RThumb1, RThumb2, RThumb3
0.0, 0.0, 0.0,
# r_gripper_joint
#0.0,
)
def __init__(self, name,
device = None,
tracer = None):
AbstractHumanoidRobot.__init__ (self, name, tracer)
print "You ask for ", name, ". "
rospack = RosPack()
self.urdfDir = rospack.get_path('romeo_description') + '/urdf/'
if name == 'romeo_small':
print "Loaded model is romeo_small.urdf."
self.urdfName = 'romeo_small.urdf'
self.halfSitting = self.halfSittingSmall
else:
print "Loaded model is romeo.urdf."
self.urdfName = 'romeo.urdf'
self.halfSitting = self.halfSittingAll
self.OperationalPoints.append('waist')
self.OperationalPoints.append('chest')
self.OperationalPointsMap = {'left-wrist' : 'LWristPitch',
'right-wrist' : 'RWristPitch',
'left-ankle' : 'LAnkleRoll',
'right-ankle' : 'RAnkleRoll',
'gaze' : 'gaze_joint',
'waist' : 'waist',
'chest' : 'TrunkYaw'}
self.device = device
# correct the name of the body link
self.dynamic = RosRobotModel("{0}_dynamic".format(name))
self.pinocchioModel = se3.buildModelFromUrdf(self.urdfDir + self.urdfName,
se3.JointModelFreeFlyer())
self.pinocchioData = self.pinocchioModel.createData()
self.dynamic.setModel(self.pinocchioModel)
self.dynamic.setData(self.pinocchioData)
# complete feet position (TODO: move it into srdf file)
#ankle =self.dynamic.getAnklePositionInFootFrame()
#self.ankleLength = 0.1935
#self.ankleWidth = 0.121
#self.dynamic.setFootParameters(True , self.ankleLength, self.ankleWidth, ankle)
#self.dynamic.setFootParameters(False, self.ankleLength, self.ankleWidth, ankle)
# check half sitting size
self.dimension = self.dynamic.getDimension()
if self.dimension != len(self.halfSitting):
raise RuntimeError("invalid half-sitting pose. HalfSitting Dimension:",
len(self.halfSitting), " .Robot Dimension:", self.dimension)
self.initializeRobot()