def createTasks(robot): # MetaTasks dictonary robot.mTasks = dict() robot.tasksIne = dict() # Foot contacts robot.contactLF = MetaTaskKine6d('contactLF',robot.dynamic,'LF', robot.OperationalPointsMap['left-ankle']) robot.contactLF.feature.frame('desired') robot.contactLF.gain.setConstant(10) robot.contactRF = MetaTaskKine6d('contactRF',robot.dynamic,'RF', robot.OperationalPointsMap['right-ankle']) robot.contactRF.feature.frame('desired') robot.contactRF.gain.setConstant(10) # MetaTasksKine6d for other operational points robot.mTasks['waist'] = MetaTaskKine6d('waist', robot.dynamic, 'waist', robot.OperationalPointsMap['waist']) robot.mTasks['chest'] = MetaTaskKine6d('chest', robot.dynamic, 'chest', robot.OperationalPointsMap['chest']) robot.mTasks['rh'] = MetaTaskKine6d('rh', robot.dynamic, 'rh', robot.OperationalPointsMap['right-wrist']) robot.mTasks['lh'] = MetaTaskKine6d('lh', robot.dynamic, 'lh', robot.OperationalPointsMap['left-wrist']) for taskName in robot.mTasks: robot.mTasks[taskName].feature.frame('desired') robot.mTasks[taskName].gain.setConstant(10) handMgrip=eye(4); handMgrip[0:3,3] = (0,0,-0.14) robot.mTasks['rh'].opmodif = matrixToTuple(handMgrip) robot.mTasks['lh'].opmodif = matrixToTuple(handMgrip) # CoM Task robot.mTasks['com'] = MetaTaskKineCom(robot.dynamic) robot.dynamic.com.recompute(0) robot.mTasks['com'].featureDes.errorIN.value = robot.dynamic.com.value robot.mTasks['com'].task.controlGain.value = 10 robot.mTasks['com'].feature.selec.value = '011' # Posture Task robot.mTasks['posture'] = MetaTaskKinePosture(robot.dynamic) robot.mTasks['posture'].ref = robot.halfSitting robot.mTasks['posture'].gain.setConstant(5) ## TASK INEQUALITY # Task Height featureHeight = FeatureGeneric('featureHeight') plug(robot.dynamic.com,featureHeight.errorIN) plug(robot.dynamic.Jcom,featureHeight.jacobianIN) robot.tasksIne['taskHeight']=TaskInequality('taskHeight') robot.tasksIne['taskHeight'].add(featureHeight.name) robot.tasksIne['taskHeight'].selec.value = '100' robot.tasksIne['taskHeight'].referenceInf.value = (0.,0.,0.) # Xmin, Ymin robot.tasksIne['taskHeight'].referenceSup.value = (0.,0.,0.80771) # Xmax, Ymax robot.tasksIne['taskHeight'].dt.value=robot.timeStep
def createTasks(robot): # MetaTasks dictonary robot.mTasks = dict() robot.tasksIne = dict() # Foot contacts robot.contactLF = MetaTaskDyn6d('contactLF', robot.dynamic, 'lf', 'left-ankle') robot.contactRF = MetaTaskDyn6d('contactRF', robot.dynamic, 'rf', 'right-ankle') setContacts(robot.contactLF, robot.contactRF) # MetaTasksDyn6d for other operational points robot.mTasks['waist'] = MetaTaskDyn6d('waist', robot.dynamic, 'waist', 'waist') robot.mTasks['chest'] = MetaTaskDyn6d('chest', robot.dynamic, 'chest', 'chest') robot.mTasks['rh'] = MetaTaskDyn6d('rh', robot.dynamic, 'rh', 'right-wrist') robot.mTasks['lh'] = MetaTaskDyn6d('lh', robot.dynamic, 'lh', 'left-wrist') for taskName in robot.mTasks: robot.mTasks[taskName].feature.frame('desired') robot.mTasks[taskName].gain.setConstant(10) robot.mTasks[taskName].task.dt.value = robot.timeStep robot.mTasks[taskName].featureDes.velocity.value = (0, 0, 0, 0, 0, 0) handMgrip = eye(4) handMgrip[0:3, 3] = (0, 0, -0.14) robot.mTasks['rh'].opmodif = matrixToTuple(handMgrip) robot.mTasks['lh'].opmodif = matrixToTuple(handMgrip) # CoM Task robot.mTasks['com'] = MetaTaskDynCom(robot.dynamic, robot.timeStep) robot.dynamic.com.recompute(0) robot.mTasks['com'].featureDes.errorIN.value = robot.dynamic.com.value robot.mTasks['com'].task.controlGain.value = 10 robot.mTasks['com'].feature.selec.value = '011' # Posture Task robot.mTasks['posture'] = MetaTaskDynPosture(robot.dynamic, robot.timeStep) robot.mTasks['posture'].ref = robot.halfSitting robot.mTasks['posture'].gain.setConstant(5) ## TASK INEQUALITY # Task Height featureHeight = FeatureGeneric('featureHeight') plug(robot.dynamic.com, featureHeight.errorIN) plug(robot.dynamic.Jcom, featureHeight.jacobianIN) robot.tasksIne['taskHeight'] = TaskDynInequality('taskHeight') plug(robot.dynamic.velocity, robot.tasksIne['taskHeight'].qdot) robot.tasksIne['taskHeight'].add(featureHeight.name) robot.tasksIne['taskHeight'].selec.value = '100' robot.tasksIne['taskHeight'].referenceInf.value = (0., 0., 0. ) # Xmin, Ymin robot.tasksIne['taskHeight'].referenceSup.value = (0., 0., 0.80771 ) # Xmax, Ymax robot.tasksIne['taskHeight'].dt.value = robot.timeStep
def test_matrix_rpy(self): for mat, rpy in [ (mod.matrixToTuple(np.identity(4)), (0, 0, 0, 0, 0, 0)), (mod.matrixToTuple(-np.identity(4)), (0, 0, 0, -np.pi, 0, -np.pi)), ]: np.testing.assert_allclose(rpy, mod.matrixToRPY(mat)) # np.testing.assert_allclose(mat, mod.RPYToMatrix(rpy)) np.testing.assert_allclose(rpy, mod.matrixToRPY(mod.RPYToMatrix(rpy))) def test_rotate(self): for axis, angle, mat in [ ('x', np.pi, ((1, 0, 0, 0), (0, -1, 0, 0), (0, 0, -1, 0), (0, 0, 0, 1))), ('y', np.pi, ((-1, 0, 0, 0), (0, 1, 0, 0), (0, 0, -1, 0), (0, 0, 0, 1))), ('z', np.pi, ((-1, 0, 0, 0), (0, -1, 0, 0), (0, 0, 1, 0), (0, 0, 0, 1))), ]: self.assertEqual(mat, mod.rotate(axis, angle)) def test_quat_mat(self): for quat, mat in [ ((0, 0, 0, 1), np.identity(3)), ((0, 0, 1, 0), ((-1, 0, 0), (0, -1, 0), (0, 0, 1))), ((0, -0.5, 0, 0.5), ((0.5, 0, -0.5), (0, 1, 0), (0.5, 0, 0.5))), ]: self.assertEqual(mat, mod.quaternionToMatrix(quat))
def goto6dRel(task,position,positionRef,gain=None,resetJacobian=True): M=generic6dReference(position) MRef=generic6dReference(positionRef) task.featureDes.position.value = matrixToTuple(M) task.featureDes.positionRef.value = matrixToTuple(MRef) task.feature.selec.value = "111111" setGain(task.gain,gain) if 'resetJacobianDerivative' in task.task.__class__.__dict__.keys() and resetJacobian: task.task.resetJacobianDerivative()
def goto6dRel(task, position, positionRef, gain=None, resetJacobian=True): M = generic6dReference(position) MRef = generic6dReference(positionRef) task.featureDes.position.value = matrixToTuple(M) task.featureDes.positionRef.value = matrixToTuple(MRef) task.feature.selec.value = Flags("111111") setGain(task.gain, gain) if 'resetJacobianDerivative' in task.task.__class__.__dict__.keys() and resetJacobian: task.task.resetJacobianDerivative()
def gotoNdRel(task,position,positionRef,selec=None,gain=None,resetJacobian=True): M=generic6dReference(position) MRef=generic6dReference(positionRef) if selec!=None: if isinstance(selec,str): task.feature.selec.value = selec else: task.feature.selec.value = toFlags(selec) task.featureDes.position.value = matrixToTuple(M) task.featureDes.positionRef.value = matrixToTuple(MRef) setGain(task.gain,gain) if 'resetJacobianDerivative' in task.task.__class__.__dict__.keys() and resetJacobian: task.task.resetJacobianDerivative()
def createTasks(robot): # MetaTasks dictonary robot.mTasks = dict() robot.tasksIne = dict() # Foot contacts robot.contactLF = MetaTaskKine6d("contactLF", robot.dynamic, "LF", "left-ankle") robot.contactLF.feature.frame("desired") robot.contactLF.gain.setConstant(10) robot.contactRF = MetaTaskKine6d("contactRF", robot.dynamic, "RF", "right-ankle") robot.contactRF.feature.frame("desired") robot.contactRF.gain.setConstant(10) # MetaTasksKine6d for other operational points robot.mTasks["waist"] = MetaTaskKine6d("waist", robot.dynamic, "waist", "waist") robot.mTasks["chest"] = MetaTaskKine6d("chest", robot.dynamic, "chest", "chest") robot.mTasks["rh"] = MetaTaskKine6d("rh", robot.dynamic, "rh", "right-wrist") robot.mTasks["lh"] = MetaTaskKine6d("lh", robot.dynamic, "lh", "left-wrist") for taskName in robot.mTasks: robot.mTasks[taskName].feature.frame("desired") robot.mTasks[taskName].gain.setConstant(10) handMgrip = eye(4) handMgrip[0:3, 3] = (0, 0, -0.14) robot.mTasks["rh"].opmodif = matrixToTuple(handMgrip) robot.mTasks["lh"].opmodif = matrixToTuple(handMgrip) # CoM Task robot.mTasks["com"] = MetaTaskKineCom(robot.dynamic) robot.dynamic.com.recompute(0) robot.mTasks["com"].featureDes.errorIN.value = robot.dynamic.com.value robot.mTasks["com"].task.controlGain.value = 10 robot.mTasks["com"].feature.selec.value = "011" # Posture Task robot.mTasks["posture"] = MetaTaskKinePosture(robot.dynamic) robot.mTasks["posture"].ref = robot.halfSitting robot.mTasks["posture"].gain.setConstant(5) ## TASK INEQUALITY # Task Height featureHeight = FeatureGeneric("featureHeight") plug(robot.dynamic.com, featureHeight.errorIN) plug(robot.dynamic.Jcom, featureHeight.jacobianIN) robot.tasksIne["taskHeight"] = TaskInequality("taskHeight") robot.tasksIne["taskHeight"].add(featureHeight.name) robot.tasksIne["taskHeight"].selec.value = "100" robot.tasksIne["taskHeight"].referenceInf.value = (0.0, 0.0, 0.0) # Xmin, Ymin robot.tasksIne["taskHeight"].referenceSup.value = (0.0, 0.0, 0.80771) # Xmax, Ymax robot.tasksIne["taskHeight"].dt.value = robot.timeStep
def gotoNdRel(task, position, positionRef, selec=None, gain=None, resetJacobian=True): M = generic6dReference(position) MRef = generic6dReference(positionRef) if selec is not None: if not isinstance(selec, Flags): selec = Flags(selec) task.feature.selec.value = selec task.featureDes.position.value = matrixToTuple(M) task.featureDes.positionRef.value = matrixToTuple(MRef) setGain(task.gain, gain) if 'resetJacobianDerivative' in task.task.__class__.__dict__.keys() and resetJacobian: task.task.resetJacobianDerivative()
def createRelativeTask(robot): robot.mTasks['rel'] = MetaTaskKine6dRel('rel', robot.dynamic, 'rh', 'lh', 'right-wrist', 'left-wrist') robot.mTasks['rel'].feature.frame('desired') robot.mTasks['rel'].gain.setConstant(10) #robot.mTasks['rel'].task.dt.value = robot.timeStep robot.mTasks['rel'].featureDes.velocity.value = (0, 0, 0, 0, 0, 0) # RELATIVE POSITION TASK handMgrip = eye(4) handMgrip[0:3, 3] = (0, 0, -0.14) robot.mTasks['rel'].opmodif = matrixToTuple(handMgrip) robot.mTasks['rel'].opmodifBase = matrixToTuple(handMgrip)
def createScrewTask(robot, TwoHandTool): refToTwoHandToolMatrix = array(RPYToMatrix(TwoHandTool)) #RH-TwoHandTool Homogeneous Transformation Matrix (fixed in time) robot.mTasks['rh'].feature.position.recompute(robot.device.control.time) RHToTwoHandToolMatrix = dot( linalg.inv(array(robot.mTasks['rh'].feature.position.value)), refToTwoHandToolMatrix) #!!!!!! RH and Support are different references, because the X rotation is not controlled in positioning!!!!!!!!!!!!!!!!!!!!!!!!!! RHToScrewMatrix = dot(RHToTwoHandToolMatrix, TwoHandToolToScrewMatrix) # Screw Lenght - unused at the moment #screw_len = 0.03 # TASK Screw robot.mTasks['screw'] = MetaTaskKine6d('screw', robot.dynamic, 'screw', 'right-wrist') handMgrip = array(robot.mTasks['rh'].opmodif) robot.mTasks['screw'].opmodif = matrixToTuple( dot(handMgrip, RHToScrewMatrix)) robot.mTasks['screw'].feature.selec.value = '000111' # TASK Screw orientation robot.mTasks['screw'].featureVec = FeatureVector3("featureVecScrew") plug(robot.mTasks['screw'].opPointModif.position, robot.mTasks['screw'].featureVec.signal('position')) plug(robot.mTasks['screw'].opPointModif.jacobian, robot.mTasks['screw'].featureVec.signal('Jq')) robot.mTasks['screw'].featureVec.vector.value = array([0., 0., 1.]) robot.mTasks['screw'].task.add(robot.mTasks['screw'].featureVec.name)
def initWaistCoMTasks(robot): # ---- TASKS ------------------------------------------------------------------- # Make sure that the CoM is not controlling the Z robot.featureCom.selec.value = '011' # Build the reference waist pos h**o-matrix from PG. # extract yaw YawFromLeftRightRPY = Multiply_matrix_vector('YawFromLeftRightRPY') YawFromLeftRightRPY.sin1.value=matrixToTuple(array([[ 0., 0., 0.], \ [ 0., 0., 0., ], [ 0., 0., 1., ]])) plug(robot.pg.signal('comattitude'), YawFromLeftRightRPY.sin2) # Build a reference vector from init waist pos and # init left foot roll pitch representation waistReferenceVector = Stack_of_vector('waistReferenceVector') plug(robot.pg.initwaistposref, waistReferenceVector.sin1) #plug(robot.pg.initwaistattref,waistReferenceVector.sin2) plug(YawFromLeftRightRPY.sout, waistReferenceVector.sin2) waistReferenceVector.selec1(0, 3) waistReferenceVector.selec2(0, 3) robot.pg.waistReference = PoseRollPitchYawToMatrixHomo('waistReference') # Controlling also the yaw. robot.waist.selec.value = '111100' robot.addTrace(robot.pg.waistReference.name, 'sout') robot.addTrace(robot.geom.name, 'position') robot.addTrace(robot.pg.name, 'initwaistposref') plug(waistReferenceVector.sout, robot.pg.waistReference.sin) plug(robot.pg.waistReference.sout, robot.waist.reference) robot.tasks['waist'].controlGain.value = 200
def initWaistCoMTasks(robot): # ---- TASKS ------------------------------------------------------------------- # Make sure that the CoM is not controlling the Z robot.featureCom.selec.value='011' # Build the reference waist pos h**o-matrix from PG. # extract yaw YawFromLeftRightRPY = Multiply_matrix_vector('YawFromLeftRightRPY') YawFromLeftRightRPY.sin1.value=matrixToTuple(array([[ 0., 0., 0.], \ [ 0., 0., 0., ], [ 0., 0., 1., ]])) plug(robot.pg.signal('comattitude'),YawFromLeftRightRPY.sin2) # Build a reference vector from init waist pos and # init left foot roll pitch representation waistReferenceVector = Stack_of_vector('waistReferenceVector') plug(robot.pg.initwaistposref,waistReferenceVector.sin1) #plug(robot.pg.initwaistattref,waistReferenceVector.sin2) plug(YawFromLeftRightRPY.sout,waistReferenceVector.sin2) waistReferenceVector.selec1(0,3) waistReferenceVector.selec2(0,3) robot.pg.waistReference=PoseRollPitchYawToMatrixHomo('waistReference') # Controlling also the yaw. robot.waist.selec.value = '111100' robot.addTrace(robot.pg.waistReference.name,'sout') robot.addTrace(robot.geom.name,'position') robot.addTrace(robot.pg.name,'initwaistposref') plug(waistReferenceVector.sout, robot.pg.waistReference.sin) plug(robot.pg.waistReference.sout,robot.waist.reference) robot.tasks ['waist'].controlGain.value = 200
def initTaskCompensate(self): # The constraint is: # cMhref !!=!! cMh = cMcc ccMh # or written in ccMh # ccMh !!=!! ccMc cMhref # c : central frame of the robot # cc : central frame for the controller (without the flexibility) # cMcc= flexibility # ccMc= flexibility inverted self.transformerR = sotso.MovingFrameTransformation('tranformation_right') self.ccMc = self.transformerR.gMl # inverted flexibility self.cMrhref = self.transformerR.lM0 # reference position in the world control frame # You need to set up the inverted flexibility : plug( ..., self.ccMc) # You need to set up a reference value here: plug( ... ,self.cMhref) self.ccVc = self.transformerR.gVl # inverted flexibility velocity self.cVrhref = self.transformerR.lV0 # reference velocity in the world control frame # You need to set up the inverted flexibility velocity : plug( ..., self.ccVc) # You need to set up a reference velocity value here: plug( ... ,self.cVhref) self.ccMrhref = self.transformerR.gM0 # reference matrix h**o in the control frame self.ccVrhref = self.transformerR.gV0 ###### self.cMrhref.value = (matrixToTuple(diag([1,1,1,1]))) self.cVrhref.value = (0.0 , 0.0 , 0.0 , 0.0 , 0.0 , 0.0)
def Pr2BaseTask(robot): task = MetaTaskKine6d('base',robot.dynamic,'waist','waist') task.opmodif = matrixToTuple(baseMground) task.feature.frame('desired') task.feature.selec.value = '011100' task.gain.setConstant(10) return task
def initTaskPosture(self): # --- LEAST NORM weight_ff = 0 weight_leg = 3 weight_knee = 5 weight_chest = 1 weight_chesttilt = 12 weight_head = 0.3 weight_arm = 0.8 weight = diag((weight_ff, ) * 6 + (weight_leg, ) * 12 + (weight_chest, ) * 2 + (weight_head, ) * 2 + (weight_arm, ) * 14) weight[9, 9] = weight_knee weight[15, 15] = weight_knee weight[19, 19] = weight_chesttilt #weight = weight[6:,:] self.featurePosture.jacobianIN.value = matrixToTuple(weight) self.featurePostureDes.errorIN.value = self.robot.halfSitting mask = '1' * 36 # mask = 6*'0'+12*'0'+4*'1'+14*'0' # mask = '000000000000111100000000000000000000000000' # robot.dynamic.displaySignals () # robot.dynamic.Jchest.value self.features['posture'].selec.value = mask
def createTasks(robot): # MetaTasks dictonary robot.mTasks = dict() robot.tasksIne = dict() # Foot contacts robot.contactLF = MetaTaskDyn6d('contactLF',robot.dynamic,'lf','left-ankle') robot.contactRF = MetaTaskDyn6d('contactRF',robot.dynamic,'rf','right-ankle') setContacts(robot.contactLF,robot.contactRF) # MetaTasksDyn6d for other operational points robot.mTasks['waist'] = MetaTaskDyn6d('waist', robot.dynamic, 'waist', 'waist') robot.mTasks['chest'] = MetaTaskDyn6d('chest', robot.dynamic, 'chest', 'chest') robot.mTasks['rh'] = MetaTaskDyn6d('rh', robot.dynamic, 'rh', 'right-wrist') robot.mTasks['lh'] = MetaTaskDyn6d('lh', robot.dynamic, 'lh', 'left-wrist') for taskName in robot.mTasks: robot.mTasks[taskName].feature.frame('desired') robot.mTasks[taskName].gain.setConstant(10) robot.mTasks[taskName].task.dt.value = robot.timeStep robot.mTasks[taskName].featureDes.velocity.value=(0,0,0,0,0,0) handMgrip=eye(4); handMgrip[0:3,3] = (0,0,-0.14) robot.mTasks['rh'].opmodif = matrixToTuple(handMgrip) robot.mTasks['lh'].opmodif = matrixToTuple(handMgrip) robot.mTasks['gaze'] = MetaTaskDynVisualPoint('gaze',robot.dynamic,'head','gaze') headMcam = array([[0.0,0.0,1.0,0.081],[1.,0.0,0.0,0.072],[0.0,1.,0.0,0.031],[0.0,0.0,0.0,1.0]]) headMcam = dot(headMcam,rotate('x',10*pi/180)) robot.mTasks['gaze'].opmodif = matrixToTuple(headMcam) robot.mTasks['gaze'].featureDes.xy.value = (0,0) robot.mTasks['gaze'].task.dt.value = robot.timeStep robot.mTasks['gaze'].gain.setConstant(1) # CoM Task robot.mTasks['com'] = MetaTaskDynCom(robot.dynamic,robot.timeStep) robot.dynamic.com.recompute(0) robot.mTasks['com'].featureDes.errorIN.value = robot.dynamic.com.value robot.mTasks['com'].task.controlGain.value = 10 robot.mTasks['com'].feature.selec.value = '011' # Posture Task robot.mTasks['posture'] = MetaTaskDynPosture(robot.dynamic,robot.timeStep) robot.mTasks['posture'].ref = robot.halfSitting robot.mTasks['posture'].gain.setConstant(5)
def screw_2ht(robot, solver, tool, target, goal, gainMax, gainMin): #goal = array([0.5,-0.3,1.1,0.,1.57,0.]) if 'rel' not in robot.mTasks: createRelativeTask(robot) if 'screw' not in robot.mTasks: createScrewTask(robot, tool) createVecMult(robot) if not hasattr(robot, 'm2pos'): createM2Pos(robot) # Task Relative gotoNdRel(robot.mTasks['rel'], array(robot.mTasks['rh'].feature.position.value), array(robot.mTasks['lh'].feature.position.value), '110111', gainMax * 2) robot.mTasks['rel'].feature.errordot.value = (0, 0, 0, 0, 0 ) # not to forget!! # Aim setting if isinstance(goal, ndarray): if len(goal) == 6: refToGoalMatrix = RPYToMatrix(goal) else: refToGoalMatrix = goal robot.mTasks['screw'].ref = matrixToTuple(refToGoalMatrix) robot.mTasks['screw'].featureVec.positionRef.value = dot( refToGoalMatrix[0:3, 0:3], array([0., 0., 1.])) else: #goal is a signal plug(goal, robot.mTasks['screw'].featureDes.position) plug(goal, robot.selec.sin) robot.mult.sin2.value = (0., 0., 1.) plug(robot.mult.sout, robot.mTasks['screw'].featureVec.positionRef) if isinstance(target, ndarray): if len(target) == 6: refToTargetMatrix = RPYToMatrix(target) else: refToTargetMatrix = target robot.mTasks['gaze'].proj.point3D.value = vectorToTuple(target[0:3, 3]) else: #target is a signal plug(target, robot.m2pos.sin) plug(robot.m2pos.sout, robot.mTasks['gaze'].proj.point3D) robot.mTasks['gaze'].gain.setByPoint(gainMax * 2, gainMin * 2, 0.01, 0.9) robot.mTasks['screw'].gain.setByPoint(gainMax, gainMin, 0.01, 0.9) tasks = array([ robot.mTasks['rel'].task, robot.mTasks['gaze'].task, robot.mTasks['screw'].task ]) # sot charging if not (('taskrel' in solver.toList()) and ('taskgaze' in solver.toList()) and ('taskscrew' in solver.toList())): removeUndesiredTasks(solver) for i in range(len(tasks)): solver.push(tasks[i])
def __init__(self,robot,taskname = 'com-stabilized'): from dynamic_graph.sot.application.state_observation.initializations.hrp2_model_base_flex_estimator import HRP2ModelBaseFlexEstimator HRP2LinearStateSpaceStabilizer.__init__(self,taskname) robot.dynamic.com.recompute(0) robot.dynamic.Jcom.recompute(0) plug (robot.dynamic.com, self.com) plug (robot.dynamic.Jcom, self.Jcom) # For determining nbSupport plug (robot.device.forceLLEG,self.force_lf) plug (robot.device.forceRLEG,self.force_rf) plug (robot.frames['rightFootForceSensor'].position,self.rightFootPosition) plug (robot.frames['leftFootForceSensor'].position,self.leftFootPosition) # Estimator of the flexibility state self.estimator = HRP2ModelBaseFlexEstimator(robot, taskname+"Estimator") plug (self.nbSupport,self.estimator.contactNbr) # In # Definition des contacts rFootPos = MatrixHomoToPoseRollPitchYaw('rFootFramePos') lFootPos = MatrixHomoToPoseRollPitchYaw('lFootFramePos') plug(robot.frames['rightFootForceSensor'].position,rFootPos.sin) plug(robot.frames['leftFootForceSensor'].position,lFootPos.sin) self.estimator.contacts = Stack_of_vector ('contacts') plug(rFootPos.sout,self.estimator.contacts.sin1) plug(lFootPos.sout,self.estimator.contacts.sin2) self.estimator.contacts.selec1 (0, 6) self.estimator.contacts.selec2 (0, 6) plug(self.estimator.contacts.sout,self.estimator.inputVector.contactsPosition) # Simulation: Stifness and damping kfe=40000 kfv=600 kte=600 ktv=60 self.estimator.setKfe(matrixToTuple(np.diag((kfe,kfe,kfe)))) self.estimator.setKfv(matrixToTuple(np.diag((kfv,kfv,kfv)))) self.estimator.setKte(matrixToTuple(np.diag((kte,kte,kte)))) self.estimator.setKtv(matrixToTuple(np.diag((ktv,ktv,ktv)))) plug(self.estimator.flexTransformationMatrix, self.stateFlex ) # Out plug(self.estimator.flexOmega, self.stateFlexDot )
def test_rpy_tr(self): for rpy, tr in [ ((0, 0, 0), mod.matrixToTuple(np.identity(4))), ((np.pi, 0, 0), ((1, 0, 0, 0), (0, -1, 0, 0), (0, 0, -1, 0), (0, 0, 0, 1))), ((0, np.pi, 0), ((-1, 0, 0, 0), (0, 1, 0, 0), (0, 0, -1, 0), (0, 0, 0, 1))), ((0, 0, np.pi), ((-1, 0, 0, 0), (0, -1, 0, 0), (0, 0, 1, 0), (0, 0, 0, 1))), ]: np.testing.assert_allclose(tr, mod.rpy2tr(*rpy), atol=1e-15)
def initTaskCompensate(self): # The constraint is: # cMhref !!=!! cMh = cMcc ccMh # or written in ccMh # ccMh !!=!! ccMc cMhref # c : central frame of the robot # cc : central frame for the controller (without the flexibility) # cMcc= flexibility # ccMc= flexibility inverted self.transformerR = sotso.MovingFrameTransformation('tranformation_right') self.ccMc = self.transformerR.gMl # inverted flexibility self.cMrhref = self.transformerR.lM0 # reference position in the world control frame # You need to set up the inverted flexibility : plug( ..., self.ccMc) # You need to set up a reference value here: plug( ... ,self.cMhref) self.ccVc = self.transformerR.gVl # inverted flexibility velocity self.cVrhref = self.transformerR.lV0 # reference velocity in the world control frame # You need to set up the inverted flexibility velocity : plug( ..., self.ccVc) # You need to set up a reference velocity value here: plug( ... ,self.cVhref) self.ccMrhref = self.transformerR.gM0 # reference matrix h**o in the control frame self.ccVrhref = self.transformerR.gV0 plug(self.ccMrhref,self.taskCompensateR.featureDes.position) plug(self.ccVrhref,self.taskCompensateR.featureDes.velocity) self.taskCompensateR.task.setWithDerivative (True) self.taskCompensateR.feature.frame('desired') ###### if self.twoHands: self.transformerL = sotso.MovingFrameTransformation('tranformation_left') plug(self.ccMrhref,self.taskCompensateL.featureDes.position) plug(self.ccVrhref,self.taskCompensateL.featureDes.velocity) self.sym = Multiply_of_matrixHomo('sym') self.sym.sin1.value =((1, 0, 0, 0), (0, -1, 0, 0), (0, 0, 1, 0), (0, 0, 0, 1)) plug (self.ccMrhref,self.sym.sin2) plug (self.sym.sout,self.taskCompensateL.featureDes.position) self.symVel = Multiply_matrix_vector('symvel') self.symVel.sin1.value =((1,0,0,0,0,0),(0,-1,0,0,0,0),(0,0,0,1,0,0),(0,0,0,1,0,0),(0,0,0,0,-1,0),(0,0,0,0,0,1)) plug (self.ccVrhref,self.symVel.sin2) plug (self.symVel.sout,self.taskCompensateL.featureDes.velocity) self.taskCompensateL.feature.selec.value='000111' self.taskCompensateL.task.setWithDerivative (True) self.taskCompensateL.feature.frame('desired') self.cMrhref.value = (matrixToTuple(diag([1,1,1,1]))) self.cVrhref.value = (0.0 , 0.0 , 0.0 , 0.0 , 0.0 , 0.0)
def gotoNd(task,position,selec=None,gain=None,resetJacobian=True): M=generic6dReference(position) if selec!=None: if isinstance(selec,str): task.feature.selec.value = selec else: task.feature.selec.value = toFlags(selec) task.featureDes.position.value = matrixToTuple(M) setGain(task.gain,gain) if 'resetJacobianDerivative' in task.task.__class__.__dict__.keys() and resetJacobian: task.task.resetJacobianDerivative()
def initWaistCoMTasks(robot): # ---- TASKS ------------------------------------------------------------------- # Make sure that the CoM is not controlling the Z robot.featureCom.selec.value = '011' # Build the reference waist pos h**o-matrix from PG. # Build a left foot roll pitch yaw representation from left foot current pos. curLeftPRPY = MatrixHomoToPoseRollPitchYaw('curLeftPRPY') plug(robot.dynamic.signal('left-ankle'), curLeftPRPY.sin) selecRPYfromCurLeftPRPY = Selec_of_vector('selecRPYfromCurLeftPRPY') selecRPYfromCurLeftPRPY.selec(3, 6) plug(curLeftPRPY.sout, selecRPYfromCurLeftPRPY.sin) curRightPRPY = MatrixHomoToPoseRollPitchYaw('curRightPRPY') plug(robot.dynamic.signal('right-ankle'), curRightPRPY.sin) selecRPYfromCurRightPRPY = Selec_of_vector('selecRPYfromCurRightPRPY') selecRPYfromCurRightPRPY.selec(3, 6) plug(curRightPRPY.sout, selecRPYfromCurRightPRPY.sin) addLeftRightRPY = Add_of_vector('addLeftRightRPY') plug(selecRPYfromCurLeftPRPY.sout, addLeftRightRPY.sin1) plug(selecRPYfromCurLeftPRPY.sout, addLeftRightRPY.sin2) mulLeftRightRPY = Multiply_double_vector('mulLeftRightRPY') mulLeftRightRPY.sin1.value = 0.5 plug(addLeftRightRPY.sout, mulLeftRightRPY.sin2) YawFromLeftRightRPY = Multiply_matrix_vector('YawFromLeftRightRPY') YawFromLeftRightRPY.sin1.value=matrixToTuple(array([[ 0., 0., 0.], \ [ 0., 0., 0., ], [ 0., 0., 1., ]])) plug(mulLeftRightRPY.sout, YawFromLeftRightRPY.sin2) # Build a reference vector from init waist pos and # init left foot roll pitch representation waistReferenceVector = Stack_of_vector('waistReferenceVector') plug(robot.pg.initwaistposref, waistReferenceVector.sin1) #plug(robot.pg.initwaistattref,waistReferenceVector.sin2) plug(YawFromLeftRightRPY.sout, waistReferenceVector.sin2) waistReferenceVector.selec1(0, 3) waistReferenceVector.selec2(0, 3) waistReference = PoseRollPitchYawToMatrixHomo('waistReference') # Controlling also the yaw. robot.waist.selec.value = '111100' robot.addTrace(waistReference.name, 'sout') robot.addTrace(robot.geom.name, 'position') robot.addTrace(robot.pg.name, 'initwaistposref') plug(waistReferenceVector.sout, waistReference.sin) plug(waistReference.sout, robot.waist.reference) robot.tasks['waist'].controlGain.value = 200
def change6dPositionReference(task,feature,gain,position,selec=None,ingain=None,resetJacobian=True): M=generic6dReference(position) if selec!=None: if isinstance(selec,str): feature.selec.value = selec else: feature.selec.value = toFlags(selec) feature.reference.value = matrixToTuple(M) if gain!=None: setGain(gain,ingain) if 'resetJacobianDerivative' in task.__class__.__dict__.keys() and resetJacobian: task.resetJacobianDerivative()
def cornersToWorldFrame(self, cornerWrtAnkle, M): ''' Input: cornerWrtAnkle: ((x1,x2,..),(y1,y2,...),(z1,z2,...)) M: worldMankle (ankle with respect to world Output: corners with respect to world: ((x1,y1,z1),(x2,y2,z2),...) ''' corner = [] for i in range(len(cornerWrtAnkle[0])): cornerWrtWorld = M*matrix((cornerWrtAnkle[0][i],cornerWrtAnkle[1][i],cornerWrtAnkle[2][i],1)).T corner.append( (matrixToTuple(cornerWrtWorld.T)[0])[0:3] ) return corner
def initWaistCoMTasks(robot): # ---- TASKS ------------------------------------------------------------------- # Make sure that the CoM is not controlling the Z robot.featureCom.selec.value='011' # Build the reference waist pos h**o-matrix from PG. # Build a left foot roll pitch yaw representation from left foot current pos. curLeftPRPY = MatrixHomoToPoseRollPitchYaw('curLeftPRPY') plug(robot.dynamic.signal('left-ankle'),curLeftPRPY.sin) selecRPYfromCurLeftPRPY = Selec_of_vector('selecRPYfromCurLeftPRPY') selecRPYfromCurLeftPRPY.selec(3,6); plug(curLeftPRPY.sout,selecRPYfromCurLeftPRPY.sin) curRightPRPY = MatrixHomoToPoseRollPitchYaw('curRightPRPY') plug(robot.dynamic.signal('right-ankle'),curRightPRPY.sin) selecRPYfromCurRightPRPY = Selec_of_vector('selecRPYfromCurRightPRPY') selecRPYfromCurRightPRPY.selec(3,6); plug(curRightPRPY.sout,selecRPYfromCurRightPRPY.sin) addLeftRightRPY = Add_of_vector('addLeftRightRPY') plug(selecRPYfromCurLeftPRPY.sout,addLeftRightRPY.sin1) plug(selecRPYfromCurLeftPRPY.sout,addLeftRightRPY.sin2) mulLeftRightRPY = Multiply_double_vector('mulLeftRightRPY') mulLeftRightRPY.sin1.value=0.5 plug(addLeftRightRPY.sout,mulLeftRightRPY.sin2) YawFromLeftRightRPY = Multiply_matrix_vector('YawFromLeftRightRPY') YawFromLeftRightRPY.sin1.value=matrixToTuple(array([[ 0., 0., 0.], \ [ 0., 0., 0., ], [ 0., 0., 1., ]])) plug(mulLeftRightRPY.sout,YawFromLeftRightRPY.sin2) # Build a reference vector from init waist pos and # init left foot roll pitch representation waistReferenceVector = Stack_of_vector('waistReferenceVector') plug(robot.pg.initwaistposref,waistReferenceVector.sin1) #plug(robot.pg.initwaistattref,waistReferenceVector.sin2) plug(YawFromLeftRightRPY.sout,waistReferenceVector.sin2) waistReferenceVector.selec1(0,3) waistReferenceVector.selec2(0,3) waistReference=PoseRollPitchYawToMatrixHomo('waistReference') # Controlling also the yaw. robot.waist.selec.value = '111100' robot.addTrace(waistReference.name,'sout') robot.addTrace(robot.geom.name,'position') robot.addTrace(robot.pg.name,'initwaistposref') plug(waistReferenceVector.sout,waistReference.sin) plug(waistReference.sout,robot.waist.reference) robot.tasks ['waist'].controlGain.value = 200
def Pr2BaseTask(robot): task = MetaTaskKine6d('base', robot.dynamic, 'waist', 'waist') baseMground = eye(4) baseMground[0:3, 3] = (0, 0, 0) task.opmodif = matrixToTuple(baseMground) task.feature.frame('desired') task.feature.selec.value = '100011' task.gain.setConstant(10) return task
def __init__(self,dyn,name="posture"): self.dyn=dyn self.name=name self.feature = FeatureGeneric('feature'+name) self.featureDes = FeatureGeneric('featureDes'+name) self.gain = GainAdaptive('gain'+name) plug(dyn.position,self.feature.errorIN) robotDim = len(dyn.position.value) self.feature.jacobianIN.value = matrixToTuple( identity(robotDim) ) self.feature.setReference(self.featureDes.name)
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)
def convertFrames(self, M, P_in): ''' the format of M is a 4x4 homogeneous transformation matrix the format of P_in is: ((x1,y1,z1), (x2,y2,z2), ... ) the format of the output is the same as the input ''' P_out = [] for i in range(len(P_in)): tmp = matrixToTuple( M*matrix(P_in[i]+(1.,)).T ) P_out.append( (tmp[0][0], tmp[1][0], tmp[2][0]) ) return tuple(P_out)
def UrFixedTask(robot): task = MetaTaskKine6d('base',robot.dynamic,'waist','waist') baseMground=eye(4); baseMground[0:3,3] = (0,0,0) task.opmodif = matrixToTuple(baseMground) task.feature.frame('desired') task.feature.selec.value = '100011' task.gain.setConstant(10) gotoNd(task,(0,0,0,0,0,0),'100011',(4.9,0.9,0.01,0.9)) return task
def __init__(self, dyn, name="posture"): self.dyn = dyn self.name = name self.feature = FeatureGeneric('feature' + name) self.featureDes = FeatureGeneric('featureDes' + name) self.gain = GainAdaptive('gain' + name) plug(dyn.position, self.feature.errorIN) robotDim = len(dyn.position.value) self.feature.jacobianIN.value = matrixToTuple(identity(robotDim)) self.feature.setReference(self.featureDes.name)
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)
def startCompensate(self): '''Start to compensate for the hand movements.''' ccMh0 = matrix(self.robot.dynamic.rh.value) self.ccMhref.sin2.value = matrixToTuple(ccMh0) print ccMh0 print print matrix(self.robot.dynamic.RF.value) self.rm(self.taskRH) self.push(self.taskCompensate)
def goto6d(task,position,gain=None): M=eye(4) if isinstance(position,matrix): position = vectorToTuple(position) if( len(position)==3 ): M[0:3,3] = position else: #print "Position 6D with rotation ... todo" # done? M = array( rpy2tr(*position[3:7]) ) M[0:3,3] = position[0:3] task.feature.selec.value = "111111" setGain(task.gain,gain) task.featureDes.position.value = matrixToTuple(M) task.task.resetJacobianDerivative()
def plugEverything(self): self.dyn.signal(self.opPoint).recompute(0) self.dyn.signal('J'+self.opPoint).recompute(0) plug(self.dyn.signal(self.opPoint),self.task.p1) plug(self.dyn.signal('J'+self.opPoint),self.task.jVel) if (self.collisionCenter != None): self.task.p2.value = matrixToTuple(self.collisionCenter) self.task.di.value = self._di self.task.ds.value = self._ds self.task.controlGain.value = self.controlGain self.task.dt.value = 0.001
def goto6d(task, position, gain=None): M = eye(4) if isinstance(position, matrix): position = vectorToTuple(position) if (len(position) == 3): M[0:3, 3] = position else: #print "Position 6D with rotation ... todo" # done? M = array(rpy2tr(*position[3:7])) M[0:3, 3] = position[0:3] task.feature.selec.value = "111111" setGain(task.gain, gain) task.featureDes.position.value = matrixToTuple(M) task.task.resetJacobianDerivative()
def Pr2FoVTask(robot, dt): task = MetaTaskVisualPoint('FoV', robot.dynamic, 'head', 'gaze') task.opmodif = matrixToTuple(Pr2headMcam) task.task = TaskInequality('taskFoVineq') task.task.add(task.feature.name) [Xmax, Ymax] = [0.38, 0.28] task.task.referenceInf.value = (-Xmax, -Ymax) # Xmin, Ymin task.task.referenceSup.value = (Xmax, Ymax) # Xmax, Ymax task.task.dt.value = dt task.task.controlGain.value = 0.01 task.featureDes.xy.value = (0, 0) return task
def Pr2FoVTask(robot,dt): task = MetaTaskVisualPoint('FoV',robot.dynamic,'head','gaze') task.opmodif = matrixToTuple(Pr2headMcam) task.task=TaskInequality('taskFoVineq') task.task.add(task.feature.name) [Xmax,Ymax]=[0.38,0.28] task.task.referenceInf.value = (-Xmax,-Ymax) # Xmin, Ymin task.task.referenceSup.value = (Xmax,Ymax) # Xmax, Ymax task.task.dt.value=dt task.task.controlGain.value=0.01 task.featureDes.xy.value = (0,0) return task
def gotoNd(task,position,selec,gain=None,resetJacobian=True): M=eye(4) if isinstance(position,matrix): position = vectorToTuple(position) if( len(position)==3 ): M[0:3,3] = position else: #print "Position 6D with rotation ... todo" # done? M = array( rpy2tr(*position[3:7]) ) M[0:3,3] = position[0:3] if isinstance(selec,str): task.feature.selec.value = selec else: task.feature.selec.value = toFlags(selec) task.featureDes.position.value = matrixToTuple(M) if resetJacobian: task.task.resetJacobianDerivative() setGain(task.gain,gain)
def setVelocityNull(self, opPointRef): ''' Project the velocity to the null space of a single foot ''' if (opPointRef == 'right-ankle'): self.sot._RF_J.recompute(self.robot.control.time) Jc = matrix(self.sot._RF_J.value) elif (opPointRef == 'left-ankle'): self.sot._LF_J.recompute(self.robot.control.time) Jc = matrix(self.sot._LF_J.value) #dq = matrix(self.sot.velocity.value).T dq = matrix(self.robot.velocity.value).T dq1 = dq - linalg.pinv(Jc)*Jc*dq self.robot.setVelocity( matrixToTuple(dq1.T)[0] )
def gotoNd(task, position, selec, gain=None, resetJacobian=True): M = eye(4) if isinstance(position, matrix): position = vectorToTuple(position) if (len(position) == 3): M[0:3, 3] = position else: #print "Position 6D with rotation ... todo" # done? M = array(rpy2tr(*position[3:7])) M[0:3, 3] = position[0:3] if isinstance(selec, str): task.feature.selec.value = selec else: task.feature.selec.value = toFlags(selec) task.featureDes.position.value = matrixToTuple(M) if resetJacobian: task.task.resetJacobianDerivative() setGain(task.gain, gain)
def __init__(self, dyn, config=None, name="config"): self.dyn = dyn self.name = name self.config = config self.feature = FeatureGeneric('feature' + name) self.featureDes = FeatureGeneric('featureDes' + name) self.gain = GainAdaptive('gain' + name) plug(dyn.position, self.feature.errorIN) robotDim = dyn.getDimension() self.feature.jacobianIN.value = matrixToTuple(identity(robotDim)) self.feature.setReference(self.featureDes.name) if config is not None: self.feature.selec.value = toFlags(self.config)
def createPostureTask(robot, taskName, ingain=1.): robot.dynamic.position.recompute(0) feature = FeatureGeneric('feature' + taskName) featureDes = FeatureGeneric('featureDes' + taskName) featureDes.errorIN.value = robot.halfSitting plug(robot.dynamic.position, feature.errorIN) feature.setReference(featureDes.name) robotDim = len(robot.dynamic.position.value) feature.jacobianIN.value = matrixToTuple(identity(robotDim)) task = Task(taskName) task.add(feature.name) gain = GainAdaptive('gain' + taskName) plug(gain.gain, task.controlGain) plug(task.error, gain.error) gain.setConstant(ingain) return (feature, featureDes, task, gain)
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)
def change6dPositionReference(task, feature, gain, position, selec=None, ingain=None, resetJacobian=True): M = generic6dReference(position) if selec != None: if isinstance(selec, str): feature.selec.value = selec else: feature.selec.value = toFlags(selec) feature.reference.value = matrixToTuple(M) if gain != None: setGain(gain, ingain) if 'resetJacobianDerivative' in task.__class__.__dict__.keys( ) and resetJacobian: task.resetJacobianDerivative()
def createPostureTask (robot, taskName, ingain = 1.): robot.dynamic.position.recompute(0) feature = FeatureGeneric('feature'+taskName) featureDes = FeatureGeneric('featureDes'+taskName) featureDes.errorIN.value = robot.halfSitting plug(robot.dynamic.position,feature.errorIN) feature.setReference(featureDes.name) robotDim = len(robot.dynamic.position.value) feature.jacobianIN.value = matrixToTuple( identity(robotDim) ) task = Task (taskName) task.add (feature.name) gain = GainAdaptive('gain'+taskName) plug(gain.gain,task.controlGain) plug(task.error,gain.error) gain.setConstant(ingain) return (feature, featureDes, task, gain)
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)
def goto6dPP(task, position, velocity, duration, current): if isinstance(position, matrix): position = vectorToTuple(position) if (len(position) == 3): ''' If only position specification ''' M = eye(4) M[0:3, 3] = position else: ''' If there is position and orientation ''' M = array(rpy2tr(*position[3:7])) M[0:3, 3] = position[0:3] task.feature.selec.value = "111111" task.task.controlSelec.value = "111111" task.featureDes.position.value = matrixToTuple(M) task.task.duration.value = duration task.task.initialTime.value = current task.task.velocityDes.value = velocity task.task.resetJacobianDerivative()
def goto6dPP(task, position, velocity, duration, current): if isinstance(position,matrix): position = vectorToTuple(position) if( len(position)==3 ): ''' If only position specification ''' M=eye(4) M[0:3,3] = position else: ''' If there is position and orientation ''' M = array( rpy2tr(*position[3:7]) ) M[0:3,3] = position[0:3] task.feature.selec.value = "111111" task.task.controlSelec.value = "111111" task.featureDes.position.value = matrixToTuple(M) task.task.duration.value = duration task.task.initialTime.value = current task.task.velocityDes.value = velocity task.task.resetJacobianDerivative()
def __init__(self, robot, name='flextimator'): DGIMUModelFreeFlexEstimation.__init__(self,name) self.setSamplingPeriod(0.005) self.robot = robot self.sensorStack = Stack_of_vector (name+'Sensors') plug(robot.device.accelerometer,self.sensorStack.sin1) plug(robot.device.gyrometer,self.sensorStack.sin2) self.sensorStack.selec1 (0, 3) self.sensorStack.selec2 (0, 3) plug(self.sensorStack.sout,self.measurement); self.inputPos = MatrixHomoToPoseUTheta(name+'InputPosition') plug(robot.frames['accelerometer'].position,self.inputPos.sin) robot.dynamic.createJacobian('ChestJ_OpPoint','chest') self.imuOpPoint = OpPointModifier('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 (robot.dynamic.chest,self.imuOpPoint.positionIN) plug (robot.dynamic.signal('ChestJ_OpPoint'),self.imuOpPoint.jacobianIN) self.inputVel = Multiply_matrix_vector(name+'InputVelocity') plug(self.imuOpPoint.jacobian,self.inputVel.sin1) plug(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.inputVector = PositionStateReconstructor (name+'EstimatorInput') plug(self.inputPosVel.sout,self.inputVector.sin) self.inputVector.inputFormat.value = '001111' self.inputVector.outputFormat.value = '011111' self.inputVector.setFiniteDifferencesInterval(2) plug(self.inputVector.sout,self.input) robot.flextimator = self
def makeTasks(self, sotrobot): from dynamic_graph.sot.core.matrix_util import matrixToTuple from dynamic_graph.sot.core import Multiply_of_matrix, Multiply_of_matrixHomo, OpPointModifier from dynamic_graph import plug if self.relative: self.graspTask = MetaTaskKine6d ( Grasp.sep + self.gripper.name + Grasp.sep + self.otherGripper.name, sotrobot.dynamic, self.gripper.sotjoint, self.gripper.sotjoint) # Creates the matrix transforming the goal gripper's position into the desired moving gripper's position: # on the other handle of the object #M = ((-1.0, 0.0, 0.0, 0.0), (0.0, 1.0, 0.0, 0.0), (0.0, 0.0, -1.0, -0.55), (0.0, 0.0, 0.0, 1.0)) M = transformToMatrix(self.otherGripper.sotpose * self.otherHandle.sotpose.inverse() * self.handle.sotpose * self.gripper.sotpose.inverse()) self.graspTask.opmodif = matrixToTuple(M) self.graspTask.feature.frame("current") setGain(self.graspTask.gain,(100,0.9,0.01,0.9)) self.graspTask.task.setWithDerivative (False) # Sets the position and velocity of the other gripper as the goal of the first gripper pose if not self.opPointExist(sotrobot.dynamic, self.otherGripper.sotjoint): sotrobot.dynamic.createOpPoint(self.otherGripper.sotjoint, self.otherGripper.sotjoint) sotrobot.dynamic.createVelocity("vel_"+self.otherGripper.sotjoint, self.otherGripper.sotjoint) plug(sotrobot.dynamic.signal(self.otherGripper.sotjoint), self.graspTask.featureDes.position) plug(sotrobot.dynamic.signal("vel_"+self.otherGripper.sotjoint), self.graspTask.featureDes.velocity) #plug(sotrobot.dynamic.signal("J"+self.otherGripper.sotjoint), self.graspTask.featureDes.Jq) # We will need a less barbaric method to do this... # Zeroes the jacobian for the joints we don't want to use. # - Create the selection matrix for the desired joints mat = () for i in range(38): mat += ((0,)*i + (1 if i in range(29, 36) else 0,) + (0,)*(37-i),) # - Multiplies it with the computed jacobian matrix of the gripper mult = Multiply_of_matrix('mult') mult.sin2.value = mat plug(self.graspTask.opPointModif.jacobian, mult.sin1) plug(mult.sout, self.graspTask.feature.Jq) self.tasks = [ self.graspTask.task ] #self.tasks = [] self += EEPosture (sotrobot, self.gripper, [-0.2 if self.closeGripper else 0])
def init_appli(robot): taskRH = MetaTaskKine6d('rh', robot.dynamic, 'rh', robot.OperationalPointsMap['right-wrist']) handMgrip = eye(4) handMgrip[0:3, 3] = (0.1, 0, 0) taskRH.opmodif = matrixToTuple(handMgrip) taskRH.feature.frame('desired') # --- STATIC COM (if not walking) taskCom = MetaTaskKineCom(robot.dynamic) robot.dynamic.com.recompute(0) taskCom.featureDes.errorIN.value = robot.dynamic.com.value taskCom.task.controlGain.value = 10 # --- CONTACTS contactLF = MetaTaskKine6d('contactLF', robot.dynamic, 'LF', robot.OperationalPointsMap['left-ankle']) contactLF.feature.frame('desired') contactLF.gain.setConstant(10) contactLF.keep() locals()['contactLF'] = contactLF contactRF = MetaTaskKine6d('contactRF', robot.dynamic, 'RF', robot.OperationalPointsMap['right-ankle']) contactRF.feature.frame('desired') contactRF.gain.setConstant(10) contactRF.keep() locals()['contactRF'] = contactRF from dynamic_graph import plug from dynamic_graph.sot.core.sot import SOT sot = SOT('sot') sot.setSize(robot.dynamic.getDimension()) plug(sot.control, robot.device.control) from dynamic_graph.ros import RosPublish ros_publish_state = RosPublish("ros_publish_state") ros_publish_state.add("vector", "state", "/sot_control/state") from dynamic_graph import plug plug(robot.device.state, ros_publish_state.state) robot.device.after.addDownsampledSignal("ros_publish_state.trigger", 100) sot.push(contactRF.task.name) sot.push(contactLF.task.name) sot.push(taskCom.task.name) robot.device.control.recompute(0)
def initTaskPosture(self): # --- LEAST NORM weight_ff = 0 weight_leg = 3 weight_knee = 5 weight_chest = 1 weight_chesttilt = 10 weight_head = 0.3 weight_arm = 1 weight = diag( (weight_ff,)*6 + (weight_leg,)*12 + (weight_chest,)*2 + (weight_head,)*2 + (weight_arm,)*14) weight[9,9] = weight_knee weight[15,15] = weight_knee weight[19,19] = weight_chesttilt weight = weight[6:,:] self.taskPosture.task.jacobian.value = matrixToTuple(weight) self.taskPosture.task.task.value = (0,)*weight.shape[0]
def gotoNdPP(task, position, velocity, selec, duration, current): if isinstance(position, matrix): position = vectorToTuple(position) if (len(position) == 3): M = eye(4) M[0:3, 3] = position else: M = array(rpy2tr(*position[3:7])) M[0:3, 3] = position[0:3] if isinstance(selec, str): task.feature.selec.value = selec task.task.controlSelec.value = selec else: task.feature.selec.value = toFlags(selec) task.task.controlSelec.value = toFlags(selec) task.featureDes.position.value = matrixToTuple(M) task.task.duration.value = duration task.task.initialTime.value = current task.task.velocityDes.value = velocity task.task.resetJacobianDerivative()
def gotoNdPP(task, position, velocity, selec, duration, current): if isinstance(position,matrix): position = vectorToTuple(position) if( len(position)==3 ): M=eye(4) M[0:3,3] = position else: M = array( rpy2tr(*position[3:7]) ) M[0:3,3] = position[0:3] if isinstance(selec,str): task.feature.selec.value = selec task.task.controlSelec.value = selec else: task.feature.selec.value = toFlags(selec) task.task.controlSelec.value = toFlags(selec) task.featureDes.position.value = matrixToTuple(M) task.task.duration.value = duration task.task.initialTime.value = current task.task.velocityDes.value = velocity task.task.resetJacobianDerivative()
def setVelocityNullAll(self): ''' Project the velocity to the null space of both feet, if they are defined as contact ''' if '_RF_p' in [s.name for s in self.sot.signals()]: self.sot._RF_J.recompute(self.robot.control.time) Jcr = matrix(self.sot._RF_J.value) if '_LF_p' in [s.name for s in self.sot.signals()]: self.sot._LF_J.recompute(self.robot.control.time) Jcl = matrix(self.sot._LF_J.value) Jc = bmat([[Jcr],[Jcl]]) else: Jc = Jcr elif '_LF_p' in [s.name for s in self.sot.signals()]: self.sot._LF_J.recompute(self.robot.control.time) Jc = matrix(self.sot._LF_J.value) dq = matrix(self.sot.velocity.value).T dq1 = dq - linalg.pinv(Jc)*Jc*dq self.robot.setVelocity( matrixToTuple(dq1.T)[0] )
def __init__(self, dyn, dt, name="posture"): self.dyn = dyn self.name = name self.feature = FeatureGeneric('feature' + name) self.featureDes = FeatureGeneric('featureDes' + name) self.task = TaskDynPD('task' + name) self.gain = GainAdaptive('gain' + name) plug(dyn.position, self.feature.errorIN) robotDim = len(dyn.position.value) self.feature.jacobianIN.value = matrixToTuple(identity(robotDim)) self.feature.setReference(self.featureDes.name) self.task.add(self.feature.name) plug(dyn.velocity, self.task.qdot) self.task.dt.value = dt plug(self.task.error, self.gain.error) plug(self.gain.gain, self.task.controlGain)
def __init__(self, dyn, joint, name=None): if name is None: name = "joint" + str(joint) self.dyn = dyn self.name = name self.joint = joint self.feature = FeatureGeneric('feature' + name) self.featureDes = FeatureGeneric('featureDes' + name) self.gain = GainAdaptive('gain' + name) self.selec = Selec_of_vector("selec" + name) self.selec.selec(joint, joint + 1) plug(dyn.position, self.selec.sin) plug(self.selec.sout, self.feature.errorIN) robotDim = len(dyn.position.value) Id = identity(robotDim) J = Id[joint:joint + 1] self.feature.jacobianIN.value = matrixToTuple(J) self.feature.setReference(self.featureDes.name)