def __init__(self, motion, yamlData):
        checkDict('error', yamlData)

        Control.__init__(self, motion, yamlData)
        self.error = (yamlData['error']['x'],
                      yamlData['error']['y'],
                      yamlData['error']['theta'])
Esempio n. 2
0
    def __init__(self, motion, yamlData):
        checkDict('tracked-body', yamlData)
        checkDict('perceived-body', yamlData)
        Control.__init__(self, motion, yamlData)

        self.corba = motion.corba
        self.robot = motion.robot
        self.trackedBody = yamlData['tracked-body']
        self.perceivedBody = yamlData['perceived-body']
Esempio n. 3
0
    def __init__(self, motion, yamlData):
        checkDict('object-name', yamlData)
        checkDict('position', yamlData)

        Control.__init__(self, motion, yamlData)

        self.robot = motion.robot

        self.objectName = yamlData['object-name']
        self.frameName = yamlData['frame-name']
        self.position = yamlData['position']

        obj = motion.environment.get(self.objectName)
        if not obj:
            raise RuntimeError('object does not exist')

        self.robotPositionFromVisp = RobotPositionFromVisp(
            'robotPositionFromViSP' + str(id(yamlData)))

        # Convert ViSP frame into usual dynamic-graph frame.
        self.robotPositionFromVisp.setSensorTransformation(
            (( 0.,  0., 1., 0.),
             ( 0., -1., 0., 0.),
             (-1.,  0., 0., 0.),
             ( 0.,  0., 0., 1.))
            )

        if motion.ros:
            self.ros = motion.ros
        else:
            self.ros = RosExport('rosExport')
        self.ros.add('matrixHomoStamped', self.objectName, self.position)

        self.robotPositionFromVisp.plannedObjectPosition.value = \
            obj.plannedPosition.dgRotationMatrix()

        plug(self.ros.signal(self.objectName),
             self.robotPositionFromVisp.cMo)
        plug(self.ros.signal(self.objectName + 'Timestamp'),
             self.robotPositionFromVisp.cMoTimestamp)

        # Plug wMc/wMr to robotPositionFromVisp
        plug(motion.robot.frames[self.frameName].position,
             self.robotPositionFromVisp.wMc)
        plug(motion.robot.dynamic.waist, self.robotPositionFromVisp.wMr)
    def __init__(self, motion, yamlData):
        checkDict('object-name', yamlData)
        checkDict('position', yamlData)

        Control.__init__(self, motion, yamlData)

        self.robot = motion.robot

        self.objectName = yamlData['object-name']
        self.position = Pose6d(yamlData['position'])

        self.virtualSensor = VirtualSensor(
            'virtualSensor' + str(id(yamlData)))

        #FIXME: should be more generic.
        feetFollower = find(lambda e: type(e) == MotionWalk, motion.motion)
        if not feetFollower:
            raise RuntimeError('control elements needs at least one ' + \
                                   'walk elelement to apply correction')

        plug(feetFollower.feetFollower.feetFollower.waist,
             self.virtualSensor.expectedRobotPosition)
        plug(motion.robot.dynamic.waist,
             self.virtualSensor.robotPosition)

        obj = motion.environment.get(self.objectName)
        if not obj:
            raise RuntimeError('object does not exist')

        self.virtualSensor.expectedObstaclePosition.value = \
            obj.plannedPosition.dgRotationMatrix()
        self.virtualSensor.obstaclePosition.value = \
            self.position.dgRotationMatrix()

        if motion.trace:
            addTrace(motion.robot, motion.trace, self.virtualSensor.name, 'position')
 def __init__(self, motion, yamlData):
     Control.__init__(self, motion, yamlData)
     raise NotImplementedError