示例#1
0
    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
        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()
示例#2
0
文件: robot.py 项目: Leiqtcv/sot_pr2
    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
        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()
示例#3
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()
示例#4
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()
示例#5
0
 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()
     self.dynamic.loadFromParameterServer()
     self.dimension = self.dynamic.getDimension()
     self.initializeUrRobot()
示例#6
0
 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()
     self.dynamic.loadFromParameterServer()
     self.dimension = self.dynamic.getDimension()
     self.initializeUrRobot()
示例#7
0
    def __init__(self, name, filename):
        AbstractHumanoidRobot.__init__(self, name)

        self.OperationalPoints.append('waist')
        p = Parser(self.name + '_dynamic', filename)
        self.dynamic = p.parse()
        self.dimension = self.dynamic.getDimension()
        if self.dimension != len(self.halfSitting):
            raise RuntimeError("invalid half-sitting pose")
        self.initializeRobot()
        self.compoundDrive = CompoundDrive(self.name + '_compoundDrive',
                                           self.device)
        self.compoundDriveTask = Task(self.name + '_compoundDriveTask')
        self.compoundDriveTask.add(self.name + '_compoundDrive')
        self.compoundDriveTask.signal('controlGain').value = 1.
示例#8
0
    def __init__(self,
                 name,
                 modelDir,
                 xmlDir,
                 device,
                 dynamicType,
                 tracer=None):
        AbstractHumanoidRobot.__init__(self, name, tracer)

        self.OperationalPoints.append('waist')
        self.OperationalPoints.append('chest')
        self.device = device
        self.modelDir = modelDir
        self.modelName = 'HRP2JRLmainsmall.wrl'
        self.specificitiesPath = xmlDir + '/HRP2SpecificitiesSmall.xml'
        self.jointRankPath = xmlDir + '/HRP2LinkJointRankSmall.xml'

        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.dynamic = self.loadModelFromJrlDynamics(self.name + '_dynamic',
                                                     modelDir, self.modelName,
                                                     self.specificitiesPath,
                                                     self.jointRankPath,
                                                     dynamicType)

        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()
示例#9
0
    def __init__(self, name, modelDir, xmlDir, device, dynamicType,
                 tracer = None):
        AbstractHumanoidRobot.__init__ (self, name, tracer)

        self.OperationalPoints.append('waist')
        self.OperationalPoints.append('chest')
        self.device = device
        self.modelDir = modelDir
        self.modelName = 'HRP2JRLmainsmall.wrl'
        self.specificitiesPath = xmlDir + '/HRP2SpecificitiesSmall.xml'
        self.jointRankPath = xmlDir + '/HRP2LinkJointRankSmall.xml'

        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.dynamic = self.loadModelFromJrlDynamics(
            self.name + '_dynamic',
            modelDir,
            self.modelName,
            self.specificitiesPath,
            self.jointRankPath,
            dynamicType)

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