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 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()