class HRP2MocapToSOT: def __init__(self, robot): self.robot = robot self.ros = RosExport('rosExportMocap') self.ros.add('matrixHomoStamped', "chest", "/evart/hrp2_head_sf/hrp2_head_sf") self.mocapFilter = MocapDataFilter('MocapDataFilter') plug(self.ros.signal('chest'), self.mocapFilter.sin) self.mocapSignal = self.mocapFilter.sout def initialize(self): m = np.matrix([ [-0.8530, 0.5208, -0.0360, 0.0049], [-0.5206, -0.8537, -0.0146, -0.0078], [-0.0384, 0.0063, 0.9993, 0.2158], [0, 0, 0, 1.0000] ]) #transformation from the chest-markers frame to the chest frame self.chestMarkersSot = OpPointModifier( 'chestMarkersSot' ) # gets the position of the chest-markers frame in sot frame self.chestMarkersSot.setEndEffector(False) self.chestMarkersSot.setTransformation(matrixToTuple(m)) plug(self.robot.dynamic.chest, self.chestMarkersSot.positionIN) self.chestMarkersSot.position.recompute(1) self.mchestsot = np.matrix(self.chestMarkersSot.position.value) self.mchestmocap = np.matrix(self.ros.signal('chest').value) self.mtransformMocap2ISot = np.linalg.inv( self.mchestsot ) * self.mchestmocap #transforms the mocap frame to the initial-sot frame self.chestMarkerMocapInISot = Multiply_of_matrixHomo( "chestMarkerMocap" ) #gets position of chest-markers frame in sot-initial frame self.chestMarkerMocapInISot.sin1.value = matrixToTuple( self.mtransformMocap2ISot) plug(self.mocapSignal, self.chestMarkerMocapInISot.sin2) self.chestMarkersSotInverse = Inverse_of_matrixHomo( "chestMarkersSotInverse" ) #inverse of the homomatrix for position of markers in local sot plug(self.chestMarkersSot.position, self.chestMarkersSotInverse.sin) self.robotPositionISot = Multiply_of_matrixHomo("robotPositionInSot") plug(self.chestMarkerMocapInISot.sout, self.robotPositionISot.sin1) plug(self.chestMarkersSotInverse.sout, self.robotPositionISot.sin2) self.robotPositionInMocap = Multiply_of_matrixHomo( "robotPositionInMocap") plug(self.mocapSignal, self.robotPositionInMocap.sin1) plug(self.chestMarkersSotInverse.sout, self.robotPositionInMocap.sin2)
class HRP2MocapToSOT: def __init__(self, robot): self.robot = robot self.ros = RosExport("rosExportMocap") self.ros.add("matrixHomoStamped", "chest", "/evart/hrp2_head_sf/hrp2_head_sf") self.mocapFilter = MocapDataFilter("MocapDataFilter") plug(self.ros.signal("chest"), self.mocapFilter.sin) self.mocapSignal = self.mocapFilter.sout def initialize(self): m = np.matrix( [ [-0.8530, 0.5208, -0.0360, 0.0049], [-0.5206, -0.8537, -0.0146, -0.0078], [-0.0384, 0.0063, 0.9993, 0.2158], [0, 0, 0, 1.0000], ] ) # transformation from the chest-markers frame to the chest frame self.chestMarkersSot = OpPointModifier( "chestMarkersSot" ) # gets the position of the chest-markers frame in sot frame self.chestMarkersSot.setEndEffector(False) self.chestMarkersSot.setTransformation(matrixToTuple(m)) plug(self.robot.dynamic.chest, self.chestMarkersSot.positionIN) self.chestMarkersSot.position.recompute(1) self.mchestsot = np.matrix(self.chestMarkersSot.position.value) self.mchestmocap = np.matrix(self.ros.signal("chest").value) self.mtransformMocap2ISot = ( np.linalg.inv(self.mchestsot) * self.mchestmocap ) # transforms the mocap frame to the initial-sot frame self.chestMarkerMocapInISot = Multiply_of_matrixHomo( "chestMarkerMocap" ) # gets position of chest-markers frame in sot-initial frame self.chestMarkerMocapInISot.sin1.value = matrixToTuple(self.mtransformMocap2ISot) plug(self.mocapSignal, self.chestMarkerMocapInISot.sin2) self.chestMarkersSotInverse = Inverse_of_matrixHomo( "chestMarkersSotInverse" ) # inverse of the homomatrix for position of markers in local sot plug(self.chestMarkersSot.position, self.chestMarkersSotInverse.sin) self.robotPositionISot = Multiply_of_matrixHomo("robotPositionInSot") plug(self.chestMarkerMocapInISot.sout, self.robotPositionISot.sin1) plug(self.chestMarkersSotInverse.sout, self.robotPositionISot.sin2) self.robotPositionInMocap = Multiply_of_matrixHomo("robotPositionInMocap") plug(self.mocapSignal, self.robotPositionInMocap.sin1) plug(self.chestMarkersSotInverse.sout, self.robotPositionInMocap.sin2)
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)
class HRP2ModelBaseFlexEstimatorIMUForce(DGIMUModelBaseFlexEstimation): def __init__(self, robot, name='flextimator', useMocap=True, dt=0.005): DGIMUModelBaseFlexEstimation.__init__(self,name) self.setSamplingPeriod(dt) self.robot = robot initDevice(self.robot) computeDynamic(self.robot,0) # Covariances self.setProcessNoiseCovariance(matrixToTuple(np.diag((1e-8,)*12+(1e-4,)*3+(1e-4,)*3+(1e-4,)*3+(1e-4,)*3+(1.e-2,)*6+(1e-15,)*2+(1.e-8,)*3))) self.setMeasurementNoiseCovariance(matrixToTuple(np.diag((1e-3,)*3+(1e-6,)*3))) self.setUnmodeledForceVariance(1e-13) self.setForceVariance(1e-4) self.setAbsolutePosVariance(1e-4) # Contact model definition # self.setKfe(matrixToTuple(np.diag((40000,40000,40000)))) self.setKfe(matrixToTuple(np.diag((150000,150000,150000)))) self.setKfv(matrixToTuple(np.diag((600,600,600)))) self.setKte(matrixToTuple(np.diag((600,600,600)))) self.setKtv(matrixToTuple(np.diag((10,10,10)))) self.setKfeCordes(matrixToTuple(np.diag((10000,10000,10000)))) self.setKfvCordes(matrixToTuple(np.diag((300,300,800)))) self.setKteCordes(matrixToTuple(np.diag((600,600,600)))) self.setKtvCordes(matrixToTuple(np.diag((60,60,60)))) # Estimator interface self.interface=EstimatorInterface(name+"EstimatorInterface") self.interface.setSamplingPeriod(dt) self.interface.setLeftHandSensorTransformation((0.,0.,1.57)) self.interface.setRightHandSensorTransformation((0.,0.,1.57)) # State and measurement definition self.interface.setWithUnmodeledMeasurements(False) self.interface.setWithModeledForces(True) self.interface.setWithAbsolutePose(False) self.setWithComBias(False) # Contacts velocities self.leftFootVelocity = Multiply_matrix_vector ('leftFootVelocity') plug(self.robot.frames['leftFootForceSensor'].jacobian,self.leftFootVelocity.sin1) plug(self.robot.device.velocity,self.leftFootVelocity.sin2) self.rightFootVelocity = Multiply_matrix_vector ('rightFootVelocity') plug(self.robot.frames['rightFootForceSensor'].jacobian,self.rightFootVelocity.sin1) plug(self.robot.device.velocity,self.rightFootVelocity.sin2) self.interface.setFDInertiaDot(True) # Contacts forces, positions and velocities # Feet plug (self.robot.device.forceLLEG,self.interface.force_lf) plug (self.robot.device.forceRLEG,self.interface.force_rf) plug (self.robot.frames['leftFootForceSensor'].position,self.interface.position_lf) plug (self.robot.frames['rightFootForceSensor'].position,self.interface.position_rf) plug (self.leftFootVelocity.sout,self.interface.velocity_lf) plug (self.rightFootVelocity.sout,self.interface.velocity_rf) # Hands plug (self.robot.device.forceLARM,self.interface.force_lh) plug (self.robot.device.forceRARM,self.interface.force_rh) plug (self.robot.dynamic.signal('right-wrist'),self.interface.position_lh) plug (self.robot.dynamic.signal('left-wrist'),self.interface.position_rh) # Strings self.Peg = (0,0,4.60) # Position of the anchorage in the global frame self.setPe(self.Peg) self.Prl1 = np.matrix([[1,0,0,0-3.19997004e-02],[0,1,0,0.15-0],[0,0,1,1.28-1],[0,0,0,1]]) # Positions of the contacts on the robot (in the local frame) with respect to the chest self.Prl2 = np.matrix([[1,0,0,0-3.19997004e-02],[0,1,0,-0.15-0],[0,0,1,1.28-1],[0,0,0,1]]) (self.contact1OpPoint,self.contact1Pos,self.contact1)=self.createContact('contact1', self.Prl1) (self.contact2OpPoint,self.contact2Pos,self.contact2)=self.createContact('contact2', self.Prl2) plug(self.contact1.sout,self.interface.position_ls) plug(self.contact2.sout,self.interface.position_rs) # Contacts model and config plug(self.interface.contactsModel,self.contactsModel) self.setWithConfigSignal(True) plug(self.interface.config,self.config) if(useMocap): # Mocap signal self.ros = RosExport('rosExportMocap') self.ros.add('matrixHomoStamped', "chest", "/evart/hrp2_14_head/hrp2_14_head") # Filtering from dynamic_graph.sot.tools import MocapDataFilter self.mocapFilter = MocapDataFilter('MocapDataFilter') plug(self.ros.signal('chest'),self.mocapFilter.sin) self.mocapSignal = self.mocapFilter.sout # Drift self.drift = DriftFromMocap(name+'Drift') plug(self.mocapSignal,self.drift.limbGlobal) plug(self.robot.dynamic.chest,self.drift.limbLocal) self.drift.init() plug(self.drift.driftInvVector,self.interface.drift) # Measurement reconstruction plug(self.robot.device.accelerometer,self.interface.accelerometer) plug(self.robot.device.gyrometer,self.interface.gyrometer) plug(self.interface.measurement,self.measurement) # Input reconstruction # IMU Vector self.inputPos = MatrixHomoToPoseUTheta(name+'InputPosition') plug(robot.frames['accelerometer'].position,self.inputPos.sin) self.robot.dynamic.createJacobian(name+'ChestJ_OpPoint','chest') self.imuOpPoint = OpPointModifier(name+'IMU_oppoint') self.imuOpPoint.setEndEffector(False) self.imuOpPoint.setTransformation(matrixToTuple(np.linalg.inv(np.matrix(self.robot.dynamic.chest.value))*np.matrix(self.robot.frames['accelerometer'].position.value))) plug (self.robot.dynamic.chest,self.imuOpPoint.positionIN) plug (self.robot.dynamic.signal(name+'ChestJ_OpPoint'),self.imuOpPoint.jacobianIN) self.inputVel = Multiply_matrix_vector(name+'InputVelocity') plug(self.imuOpPoint.jacobian,self.inputVel.sin1) plug(self.robot.device.velocity,self.inputVel.sin2) self.inputPosVel = Stack_of_vector (name+'InputPosVel') plug(self.inputPos.sout,self.inputPosVel.sin1) plug(self.inputVel.sout,self.inputPosVel.sin2) self.inputPosVel.selec1 (0, 6) self.inputPosVel.selec2 (0, 6) self.IMUVector = PositionStateReconstructor (name+'EstimatorInput') self.IMUVector.setSamplingPeriod(dt) plug(self.inputPosVel.sout,self.IMUVector.sin) self.IMUVector.inputFormat.value = '001111' self.IMUVector.outputFormat.value = '011111' self.IMUVector.setFiniteDifferencesInterval(1) self.inputPosVel.sout.recompute(0) self.IMUVector.setLastVector(self.inputPosVel.sout.value+(0.,)*6) # CoM and derivatives self.com=self.robot.dynamic.com self.DCom = Multiply_matrix_vector(name+'DCom') plug(self.robot.dynamic.Jcom,self.DCom.sin1) plug(self.robot.device.velocity,self.DCom.sin2) self.comVectorIn = Stack_of_vector (name+'ComVectorIn') plug(self.com,self.comVectorIn.sin1) plug(self.DCom.sout,self.comVectorIn.sin2) self.comVectorIn.selec1 (0, 3) self.comVectorIn.selec2 (0, 3) self.comVector = PositionStateReconstructor (name+'ComVector') self.comVector.setSamplingPeriod(dt) plug(self.comVectorIn.sout,self.comVector.sin) self.comVector.inputFormat.value = '000101' self.comVector.outputFormat.value = '010101' self.comVector.setFiniteDifferencesInterval(1) self.DCom.sout.recompute(0) self.comVector.setLastVector(self.com.value+(0.,)*15)#(0.,)*3+self.DCom.sout.value+(0.,)*9) # Compute derivative of Angular Momentum self.angMomDerivator = Derivator_of_Vector(name+'angMomDerivator') plug(self.robot.dynamic.angularmomentum,self.angMomDerivator.sin) self.angMomDerivator.dt.value = dt # self.angMomDerivator = PositionStateReconstructor (name+'angMomDerivator') # self.angMomDerivator.setSamplingPeriod(dt) # plug(self.robot.dynamic.angularmomentum,self.angMomDerivator.sin) # self.angMomDerivator.inputFormat.value = '000001' # self.angMomDerivator.outputFormat.value = '000100' # self.angMomDerivator.setFiniteDifferencesInterval(2) # self.robot.dynamic.angularmomentum.recompute(0) # self.angMomDerivator.setLastVector(self.robot.dynamic.angularmomentum.value+(0.,)*15) # Concatenate with interface estimator plug(self.comVector.sout,self.interface.comVector) plug(self.robot.dynamic.inertia,self.interface.inertia) self.interface.dinertia.value=(0,0,0,0,0,0) plug(self.robot.dynamic.angularmomentum,self.interface.angMomentum) plug(self.angMomDerivator.sout,self.interface.dangMomentum) plug(self.robot.dynamic.waist,self.interface.positionWaist) plug(self.IMUVector.sout,self.interface.imuVector) plug(self.interface.input,self.input) plug (self.interface.modeledContactsNbr,self.contactNbr) self.robot.flextimator = self def initAbsolutePoses(self): self.drift.init() def createContact(self,name, prl): self.contactOpPoint = OpPointModifier(name+'_opPoint') self.contactOpPoint.setEndEffector(False) self.contactOpPoint.setTransformation(matrixToTuple(prl)) plug (self.robot.dynamic.chest,self.contactOpPoint.positionIN) self.contactPos = MatrixHomoToPose(name+'_pos') plug(self.contactOpPoint.position, self.contactPos.sin) self.contact = Stack_of_vector (name) plug(self.contactPos.sout,self.contact.sin1) self.contact.sin2.value = (0,0,0) self.contact.selec1 (0, 3) self.contact.selec2 (0, 3) return (self.contactOpPoint,self.contactPos,self.contact)
class HRP2ModelBaseFlexEstimatorIMUForce(DGIMUModelBaseFlexEstimation): def __init__(self, robot, name='flextimator', useMocap=True, dt=0.005): DGIMUModelBaseFlexEstimation.__init__(self,name) self.setSamplingPeriod(dt) self.robot = robot initDevice(self.robot) computeDynamic(self.robot,0) # Covariances self.setProcessNoiseCovariance(matrixToTuple(np.diag((1e-8,)*12+(1e-4,)*3+(1e-4,)*3+(1e-4,)*3+(1e-4,)*3+(1.e-2,)*6+(1e-15,)*2+(1.e-8,)*3))) self.setMeasurementNoiseCovariance(matrixToTuple(np.diag((1e-3,)*3+(1e-6,)*3))) self.setUnmodeledForceVariance(1e-13) self.setForceVariance(1e-4) self.setAbsolutePosVariance(1e-4) # Contact model definition # self.setKfe(matrixToTuple(np.diag((40000,40000,40000)))) self.setKfe(matrixToTuple(np.diag((150000,150000,150000)))) self.setKfv(matrixToTuple(np.diag((600,600,600)))) self.setKte(matrixToTuple(np.diag((600,600,600)))) self.setKtv(matrixToTuple(np.diag((10,10,10)))) self.setKfeRopes(matrixToTuple(np.diag((10000,10000,10000)))) self.setKfvRopes(matrixToTuple(np.diag((300,300,800)))) self.setKteRopes(matrixToTuple(np.diag((600,600,600)))) self.setKtvRopes(matrixToTuple(np.diag((60,60,60)))) # Estimator interface self.interface=EstimatorInterface(name+"EstimatorInterface") self.interface.setSamplingPeriod(dt) self.interface.setLeftHandSensorTransformation((0.,0.,1.57)) self.interface.setRightHandSensorTransformation((0.,0.,1.57)) # State and measurement definition self.interface.setWithUnmodeledMeasurements(False) self.interface.setWithModeledForces(True) self.interface.setWithAbsolutePose(False) self.setWithComBias(False) # Contacts velocities self.leftFootVelocity = Multiply_matrix_vector ('leftFootVelocity') plug(self.robot.frames['leftFootForceSensor'].jacobian,self.leftFootVelocity.sin1) plug(self.robot.device.velocity,self.leftFootVelocity.sin2) self.rightFootVelocity = Multiply_matrix_vector ('rightFootVelocity') plug(self.robot.frames['rightFootForceSensor'].jacobian,self.rightFootVelocity.sin1) plug(self.robot.device.velocity,self.rightFootVelocity.sin2) self.interface.setFDInertiaDot(True) # Contacts forces, positions and velocities # Feet plug (self.robot.device.forceLLEG,self.interface.force_lf) plug (self.robot.device.forceRLEG,self.interface.force_rf) plug (self.robot.frames['leftFootForceSensor'].position,self.interface.position_lf) plug (self.robot.frames['rightFootForceSensor'].position,self.interface.position_rf) plug (self.leftFootVelocity.sout,self.interface.velocity_lf) plug (self.rightFootVelocity.sout,self.interface.velocity_rf) # Hands plug (self.robot.device.forceLARM,self.interface.force_lh) plug (self.robot.device.forceRARM,self.interface.force_rh) plug (self.robot.dynamic.signal('right-wrist'),self.interface.position_lh) plug (self.robot.dynamic.signal('left-wrist'),self.interface.position_rh) # Strings self.Peg = (0,0,4.60) # Position of the anchorage in the global frame self.setPe(self.Peg) self.Prl1 = np.matrix([[1,0,0,0-3.19997004e-02],[0,1,0,0.15-0],[0,0,1,1.28-1],[0,0,0,1]]) # Positions of the contacts on the robot (in the local frame) with respect to the chest self.Prl2 = np.matrix([[1,0,0,0-3.19997004e-02],[0,1,0,-0.15-0],[0,0,1,1.28-1],[0,0,0,1]]) (self.contact1OpPoint,self.contact1Pos,self.contact1)=self.createContact('contact1', self.Prl1) (self.contact2OpPoint,self.contact2Pos,self.contact2)=self.createContact('contact2', self.Prl2) plug(self.contact1.sout,self.interface.position_ls) plug(self.contact2.sout,self.interface.position_rs) # Contacts model and config plug(self.interface.contactsModel,self.contactsModel) self.setWithConfigSignal(True) plug(self.interface.config,self.config) if(useMocap): # Mocap signal self.ros = RosExport('rosExportMocap') self.ros.add('matrixHomoStamped', "chest", "/evart/hrp2_14_head/hrp2_14_head") # Filtering from dynamic_graph.sot.tools import MocapDataFilter self.mocapFilter = MocapDataFilter('MocapDataFilter') plug(self.ros.signal('chest'),self.mocapFilter.sin) self.mocapSignal = self.mocapFilter.sout # Drift self.drift = DriftFromMocap(name+'Drift') plug(self.mocapSignal,self.drift.limbGlobal) plug(self.robot.dynamic.chest,self.drift.limbLocal) self.drift.init() plug(self.drift.driftInvVector,self.interface.drift) # Measurement reconstruction plug(self.robot.device.accelerometer,self.interface.accelerometer) plug(self.robot.device.gyrometer,self.interface.gyrometer) plug(self.interface.measurement,self.measurement) # Input reconstruction # IMU Vector self.inputPos = MatrixHomoToPoseUTheta(name+'InputPosition') plug(robot.frames['accelerometer'].position,self.inputPos.sin) self.robot.dynamic.createJacobian(name+'ChestJ_OpPoint','chest') self.imuOpPoint = OpPointModifier(name+'IMU_oppoint') self.imuOpPoint.setEndEffector(False) self.imuOpPoint.setTransformation(matrixToTuple(np.linalg.inv(np.matrix(self.robot.dynamic.chest.value))*np.matrix(self.robot.frames['accelerometer'].position.value))) plug (self.robot.dynamic.chest,self.imuOpPoint.positionIN) plug (self.robot.dynamic.signal(name+'ChestJ_OpPoint'),self.imuOpPoint.jacobianIN) self.inputVel = Multiply_matrix_vector(name+'InputVelocity') plug(self.imuOpPoint.jacobian,self.inputVel.sin1) plug(self.robot.device.velocity,self.inputVel.sin2) self.inputPosVel = Stack_of_vector (name+'InputPosVel') plug(self.inputPos.sout,self.inputPosVel.sin1) plug(self.inputVel.sout,self.inputPosVel.sin2) self.inputPosVel.selec1 (0, 6) self.inputPosVel.selec2 (0, 6) self.IMUVector = PositionStateReconstructor (name+'EstimatorInput') self.IMUVector.setSamplingPeriod(dt) plug(self.inputPosVel.sout,self.IMUVector.sin) self.IMUVector.inputFormat.value = '001111' self.IMUVector.outputFormat.value = '011111' self.IMUVector.setFiniteDifferencesInterval(1) self.inputPosVel.sout.recompute(0) self.IMUVector.setLastVector(self.inputPosVel.sout.value+(0.,)*6) # CoM and derivatives self.com=self.robot.dynamic.com self.DCom = Multiply_matrix_vector(name+'DCom') plug(self.robot.dynamic.Jcom,self.DCom.sin1) plug(self.robot.device.velocity,self.DCom.sin2) self.comVectorIn = Stack_of_vector (name+'ComVectorIn') plug(self.com,self.comVectorIn.sin1) plug(self.DCom.sout,self.comVectorIn.sin2) self.comVectorIn.selec1 (0, 3) self.comVectorIn.selec2 (0, 3) self.comVector = PositionStateReconstructor (name+'ComVector') self.comVector.setSamplingPeriod(dt) plug(self.comVectorIn.sout,self.comVector.sin) self.comVector.inputFormat.value = '000101' self.comVector.outputFormat.value = '010101' self.comVector.setFiniteDifferencesInterval(1) self.DCom.sout.recompute(0) self.comVector.setLastVector(self.com.value+(0.,)*15)#(0.,)*3+self.DCom.sout.value+(0.,)*9) # Compute derivative of Angular Momentum self.angMomDerivator = Derivator_of_Vector(name+'angMomDerivator') plug(self.robot.dynamic.angularmomentum,self.angMomDerivator.sin) self.angMomDerivator.dt.value = dt # self.angMomDerivator = PositionStateReconstructor (name+'angMomDerivator') # self.angMomDerivator.setSamplingPeriod(dt) # plug(self.robot.dynamic.angularmomentum,self.angMomDerivator.sin) # self.angMomDerivator.inputFormat.value = '000001' # self.angMomDerivator.outputFormat.value = '000100' # self.angMomDerivator.setFiniteDifferencesInterval(2) # self.robot.dynamic.angularmomentum.recompute(0) # self.angMomDerivator.setLastVector(self.robot.dynamic.angularmomentum.value+(0.,)*15) # Concatenate with interface estimator plug(self.comVector.sout,self.interface.comVector) plug(self.robot.dynamic.inertia,self.interface.inertia) self.interface.dinertia.value=(0,0,0,0,0,0) plug(self.robot.dynamic.angularmomentum,self.interface.angMomentum) plug(self.angMomDerivator.sout,self.interface.dangMomentum) plug(self.robot.dynamic.waist,self.interface.positionWaist) plug(self.IMUVector.sout,self.interface.imuVector) plug(self.interface.input,self.input) plug (self.interface.modeledContactsNbr,self.contactNbr) self.robot.flextimator = self def initAbsolutePoses(self): self.drift.init() def createContact(self,name, prl): self.contactOpPoint = OpPointModifier(name+'_opPoint') self.contactOpPoint.setEndEffector(False) self.contactOpPoint.setTransformation(matrixToTuple(prl)) plug (self.robot.dynamic.chest,self.contactOpPoint.positionIN) self.contactPos = MatrixHomoToPose(name+'_pos') plug(self.contactOpPoint.position, self.contactPos.sin) self.contact = Stack_of_vector (name) plug(self.contactPos.sout,self.contact.sin1) self.contact.sin2.value = (0,0,0) self.contact.selec1 (0, 3) self.contact.selec2 (0, 3) return (self.contactOpPoint,self.contactPos,self.contact)
class MotionPlan(object): robot = None solver = None feetFollower = None corba = None ros = None plan = None duration = 0 motion = [] control = [] footsteps = [] environment = {} trace = None started = False maxX = FeetFollowerGraphWithCorrection.maxX maxY = FeetFollowerGraphWithCorrection.maxY maxTheta = FeetFollowerGraphWithCorrection.maxTheta def __init__(self, filename, robot, solver, defaultDirectories, logger = None): if not logger: logger = initializeLogging() self.defaultDirectories = defaultDirectories self.robot = robot self.solver = solver self.logger = logger self.logger.info('loading motion plan file \'{0}\''.format(filename)) self.plan = yaml.load( open(searchFile(filename, defaultDirectories), "r")) self.duration = float(self.plan['duration']) # Middleware proxies. self.corba = CorbaServer('corba_server') self.ros = RosExport('rosExport') # Supervisor. self.supervisor = Supervisor('supervisor') self.robot.device.after.addSignal(self.supervisor.name + '.trigger') self.supervisor.setSolver(self.solver.sot.name) # Load plan. self.logger.debug('loading environment') self.loadEnvironment() self.logger.debug('loading motion elements') self.loadMotion() self.logger.debug('loading control elements') self.loadControl() # For now, only 1 feet follower is allowed (must start at t=0). feetFollowerElement = find(lambda e: type(e) == MotionWalk, self.motion) hasControl = len(self.control) > 0 self.supervisor.setPostureFeature( feetFollowerElement.feetFollower.postureFeature.name) # Plug motion signals which depend on control. for m in self.motion: if type(m) == MotionVisualPoint: # FIXME: this is so wrong. plug(self.ros.signal(m.objectName), m.vispPointProjection.cMo) plug(self.ros.signal(m.objectName + 'Timestamp'), m.vispPointProjection.cMoTimestamp) if hasControl and feetFollowerElement: self.feetFollower = FeetFollowerGraphWithCorrection( robot, solver, feetFollowerElement.feetFollower, MotionPlanErrorEstimationStrategy, maxX = self.maxX, maxY = self.maxY, maxTheta = self.maxTheta) self.feetFollower.errorEstimationStrategy.motionPlan = self #FIXME: not enough generic self.feetFollower.feetFollower.setFootsteps( 2., makeFootsteps(self.footsteps)) elif feetFollowerElement: self.feetFollower = feetFollowerElement.feetFollower else: self.feetFollower = None self.logger.debug('motion plan created with success') def loadEnvironment(self): if not 'environment' in self.plan: return for obj in self.plan['environment']: checkDict('object', obj) checkDict('name', obj['object']) self.environment[obj['object']['name']] = \ EnvironmentObject(self, obj['object']) self.logger.debug('adding object \'{0}\''.format(obj['object']['name'])) def loadMotion(self): if not 'motion' in self.plan or not self.plan['motion']: return if 'maximum-correction-per-step' in self.plan: self.maxX = self.plan['maximum-correction-per-step']['x'] self.maxY = self.plan['maximum-correction-per-step']['y'] self.maxTheta = self.plan['maximum-correction-per-step']['theta'] motionClasses = [MotionWalk, MotionJoint, MotionTask, MotionVisualPoint] for motion in self.plan['motion']: if len(motion.items()) != 1: raise RuntimeError('each motion should have only one type') (tag, data) = motion.items()[0] cls = find(lambda c: c.yaml_tag == tag, motionClasses) if not cls: raise RuntimeError('invalid motion element') self.motion.append(cls(self, data, self.defaultDirectories)) self.logger.debug('adding motion element \'{0}\''.format(tag)) if self.trace: for motion in self.motion: motion.setupTrace(self.trace) def loadControl(self): if not 'control' in self.plan or not self.plan['control']: return controlClasses = [ControlConstant, ControlMocap, ControlViSP, ControlHueblob, ControlVirtualSensor] for control in self.plan['control']: if len(control.items()) != 1: raise RuntimeError('each control should have only one type') (tag, data) = control.items()[0] cls = find(lambda c: c.yaml_tag == tag, controlClasses) if not cls: raise RuntimeError('invalid control element') self.control.append(cls(self, data)) self.logger.debug('adding control element \'{0}\''.format(tag)) def __str__(self): res = 'Motion:\n' res += '-------\n' for motion in self.motion: res += '\t* {0}\n'.format(str(motion)) res += '\n' res += 'Control:\n' res += '--------\n' for control in self.control: res += '\t* {0}\n'.format(str(control)) res += '\n' res += str(self.feetFollower) res += '\n\n' res += self.supervisor.display() return res def start(self): if self.started: self.logger.info('already started') return if not self.canStart(): self.logger.info('failed to start') return self.started = True self.logger.info('execution starts') if self.feetFollower: self.feetFollower.start() # Remove default tasks and let the supervisor take over the # tasks management. self.solver.sot.clear() tOrigin = self.feetFollower.feetFollower.getStartTime() self.supervisor.setOrigin(max(0., tOrigin)) def canStart(self): canStart = reduce(lambda acc, c: c.canStart() and acc, self.control, True) if not canStart: return False if self.feetFollower: return self.feetFollower.canStart() else: return True