def test_ros(robot): # SCRIPT PARAMETERS j = 0 # index of joint under analysis N = 300 # test duration (in number of timesteps) dt = 0.001 # time step estimationDelay = 10 # delay introduced by the estimation [number of time steps] createRosTopics = 1 # 1=true, 0=false # CONSTANTS nj = 30 # number of joints rad2deg = 180 / 3.14 app = Application(robot) dq_des = nj * (0.0, ) app.robot.device.control.value = dq_des if (createRosTopics == 1): ros = RosExport('rosExport') ros.add('vector', 'robotStateRos', 'state') plug(robot.device.state, ros.robotStateRos) # robot.device.after.addSignal('rosExport.robotStateRos') return ros
def test_ros(robot): # SCRIPT PARAMETERS j = 0; # index of joint under analysis N = 300; # test duration (in number of timesteps) dt = 0.001; # time step estimationDelay = 10; # delay introduced by the estimation [number of time steps] createRosTopics = 1; # 1=true, 0=false # CONSTANTS nj = 30; # number of joints rad2deg = 180/3.14; app = Application(robot); dq_des = nj*(0.0,); app.robot.device.control.value = dq_des; if(createRosTopics==1): ros = RosExport('rosExport'); ros.add('vector', 'robotStateRos', 'state'); plug(robot.device.state, ros.robotStateRos); # robot.device.after.addSignal('rosExport.robotStateRos') return ros
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)
#connect to mocap from dynamic_graph.ros import RosExport rosIn = RosExport('rosExportMocap') rosIn.add('matrixHomoStamped', "chest", "/evart/chest9M/chest9M") robot.device.after.addDownsampledSignal('rosExportMocap.chest',1) rosIn.add('matrixHomoStamped', "left_foot", "/evart/hrp2_14_LeftFoot/hrp2_14_LeftFoot") robot.device.after.addDownsampledSignal('rosExportMocap.left_foot',1) rosIn.add('matrixHomoStamped', "left_hand", "/evart/hand_flag4m/hand_flag4m") robot.device.after.addDownsampledSignal('rosExportMocap.left_hand',1) #republish mocap ros.add('matrix', 'chest_ros', 'chest'); plug(rosIn.chest, ros.chest_ros); tracer=create_tracer(robot.device) addTrace(tracer,rosIn,'chest') addTrace(tracer,rosIn,'left_foot') addTrace(tracer,rosIn,'left_hand') #MOCAP:************************************** ssh tflayols@hakusan-vm export ROS_MASTER_URI=http://hrp2014c:11311 roslaunch evart_bridge start.launch& rosservice call /evart/list_segments rosservice call /evart/track_segments chest9M chest9M rosservice call /evart/track_segments hand_flag4m hand_flag4m rosservice call /evart/track_segments hrp2_14_LeftFoot hrp2_14_LeftFoot
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)