def __init__(self, motion, yamlData):
checkDict('error', yamlData)
Control.__init__(self, motion, yamlData)
self.error = (yamlData['error']['x'],
yamlData['error']['y'],
yamlData['error']['theta'])
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']
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
