示例#1
0
    def __init__(self, name, device=None, tracer=None):
        AbstractHumanoidRobot.__init__(self, name, tracer)
        self.urdfDir = 'package://romeo_description/urdf/'
        self.urdfName = 'romeo_small.urdf'

        self.OperationalPoints.append('waist')
        self.OperationalPoints.append('chest')
        self.device = device

        # correct the name of the body link
        self.dynamic = RosRobotModel("{0}_dynamic".format(name))
        for i in self.jointMap:
            self.dynamic.addJointMapping(i, self.jointMap[i])
        self.dynamic.loadUrdf(self.urdfDir + self.urdfName)

        # 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")
        self.initializeRobot()
示例#2
0
文件: robot.py 项目: proyan/sot-romeo
    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 项目: nim65s/sot-hrp2
    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()