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