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)
class ControlViSP(Control): yaml_tag = u'visp' 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 start(self, name, feetFollowerWithCorrection): I = ((1.,0.,0.,0.), (0.,1.,0.,0.), (0.,0.,1.,0.), (0.,0.,0.,1.)) self.estimator = ErrorEstimator(name) self.estimator.setReferenceTrajectory( feetFollowerWithCorrection.referenceTrajectory.name) plug(feetFollowerWithCorrection.referenceTrajectory.waist, self.estimator.planned) self.estimator.setSensorToWorldTransformation(I) #FIXME: we should change the reference point accordingly # with the current contact point. plug(self.robot.dynamic.signal('Jleft-ankle'), self.estimator.referencePointJacobian) self.estimator.plannedCommand.value = self.robot.device.state.value if type(self.robot.device) == RobotSimu: self.estimator.realCommand.value = self.robot.device.state.value else: self.estimator.realCommand.value = self.robot.device.robotState.value plug(self.robotPositionFromVisp.position, self.estimator.position) plug(self.robotPositionFromVisp.positionTimestamp, self.estimator.positionTimestamp) self.setupTrace(self.estimator) return self.estimator def interactiveStart(self, name, feetFollowerWithCorrection): while len(self.ros.signals()) == 0: raw_input("Press enter after starting ROS visp_tracker node.") while len(self.ros.signal(self.objectName).value) < 1: raw_input("Tracking not started...") return self.start(name, feetFollowerWithCorrection) def canStart(self): if not self.ros: return False if len(self.ros.signals()) == 0: return False if len(self.ros.signal(self.objectName).value) < 1: return False if len(self.ros.signal(self.objectName + 'Timestamp').value) < 1: return False return True def setupTrace(self, errorEstimator): self.setupTraceErrorEstimator(self.estimator) for s in [self.objectName, self.objectName + 'Timestamp']: addTrace(self.robot, self.trace, self.ros.name, s) for s in ['cMo', 'cMoTimestamp', 'plannedObjectPosition', 'position', 'positionTimestamp', 'dbgcMo', 'dbgPosition', 'dbgrMc']: addTrace(self.robot, self.trace, self.robotPositionFromVisp.name, s) def __str__(self): msg = "ViSP moving edge tracker control element (frame: {0}, object: {1})" return msg.format(self.frameName, self.objectName)
# as published by the Free Software Foundation, either version 3 of # the License, or (at your option) any later version. # # dynamic-graph is distributed in the hope that it will be useful, but # WITHOUT ANY WARRANTY; without even the implied warranty of # MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU # General Lesser Public License for more details. You should have # received a copy of the GNU Lesser General Public License along with # dynamic-graph. If not, see <http://www.gnu.org/licenses/>. import numpy as np from dynamic_graph.sot.motion_planner.feet_follower import \ RobotPositionFromVisp rpfv = RobotPositionFromVisp('rpfv') rpfv.setSensorTransformation( (( 0., 0., 1., 0.), ( 0., -1., 0., 0.), (-1., 0., 0., 0.), ( 0., 0., 0., 1.)) ) print("local to world:\n" + str(np.matrix( (( 0., 0., 1., 0.), ( 0., -1., 0., 0.), (-1., 0., 0., 0.), ( 0., 0., 0., 1.)) ))) def printDbg():