예제 #1
0
파일: hrp2.py 프로젝트: nim65s/sot-hrp2
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()
예제 #2
0
파일: robot.py 프로젝트: proyan/sot-romeo
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()
예제 #3
0
파일: hrp2.py 프로젝트: proyan/sot-hrp2
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()
예제 #4
0
파일: robot.py 프로젝트: proyan/sot-romeo
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()