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 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 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 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()
appli.features['chest'].frame('desired') appli.features['waist'].frame('desired') appli.features['gaze'].frame('desired') appli.features['chest'].selec.value = '010000' appli.features['waist'].selec.value = '010000' appli.features['gaze'].selec.value = '111000' appli.solver.push(trunktask) appli.solver.push(appli.tasks["right-wrist"]) appli.solver.push(appli.tasks["left-wrist"]) #tasks definition appli.features['right-wrist'].reference.value = matrixToTuple(generic6dReference((0.10,-0.25,0.85,0,-pi/2,0))) #appli.features['right-wrist'].reference.value = matrixToTuple(generic6dReference((0.35,-0.25,0.85,0,-pi/2,0))) appli.features['left-wrist'].reference.value = matrixToTuple(generic6dReference((0.10,0.25,0.85,0,-pi/2,0))) #appli.features['left-wrist'].reference.value = matrixToTuple(generic6dReference((0.35,0.25,0.85,0,-pi/2,0))) appli.features['left-wrist'].reference.value = matrixToTuple(generic6dReference((0.10,0.25,0.85,pi/2,0,-pi/2))) appli.features['left-ankle'].reference.value = matrixToTuple(generic6dReference((0.015,0.25,0.2,0,0,0))) appli.features['right-ankle'].reference.value = matrixToTuple(generic6dReference((0.015,-0.25,0.2,0,0,0))) #retirer tache du solver appli.solver.sot.remove('trunk-task')
hl = (2e10,2e10,2e10,2e10,2e10,2e10,5.84014,2.61799,-0.015708,3.4292,5.64159,0,0) taskjl = TaskJointLimits('taskJL') plug(robot.dynamic.position,taskjl.position) taskjl.controlGain.value = 10 taskjl.referenceInf.value = ll taskjl.referenceSup.value = hl taskjl.dt.value = 1 #armtask task_wrist_metakine=MetaTaskKine6d('task_wrist_metakine',robot.dynamic,'arm_joint_5','arm_joint_5') ip = (0.200, -0.006, 0.377,-0.155, 0.414, -0.555) zp = (0.198, 0.001, 0.581 ,-0.000, 0.001, 0.010) prl = (0.606, -0.001, 0.425,-3.115, 0.228, -3.029) aov = (-0.136, -0.005, 0.389,0.005, -0.575, 0.010) pp = (0.567, -0.013, 0.260,0,0,0) pr = (0.008,0.04,0.515,0,0,0) goal = matrixToTuple(generic6dReference(zp)) print goal #solver.damping.value =3e-2 task_wrist_metakine.gain.setConstant(1) task_wrist_metakine.feature.selec.value = '000111' task_wrist_metakine.featureDes.position.value = goal #solver #solver.push (task_waist.name) solver.push (taskjl.name) solver.push (task_waist_metakine.task.name) solver.push (task_wrist_metakine.task.name) plug (solver.control,robot.device.control) dt = 0.01 from dynamic_graph.sot.core.utils.thread_interruptible_loop import loopInThread,loopShortcuts
# tracer robot.initializeTracer() dimension = robot.dynamic.getDimension() robot.device.resize (dimension) robot.startTracer () robot_pose = ((1.,0,0,0.0),(0,1.,0,0.0),(0,0,1.,0),(0,0,0,1.),) feature_waist = FeaturePosition ('waist_feature', robot.dynamic.base_joint, robot.dynamic.Jbase_joint, robot_pose) feature_waist.selec.value = '011100' task_waist = Task ('waist_task') task_waist.controlGain.value = 10 task_waist.add (feature_waist.name) ip = (0.200, -0.006, 0.377,-0.155, 0.414, -0.555) zp = (0.198, 0.001, 0.581 ,-0.000, 0.001, 0.010) prl = (1.006, -0.001, 0.125,-3.115, 0.228, -3.029) aov = (-0.136, -0.005, 0.389,0.005, -0.575, 0.010) goal = matrixToTuple(generic6dReference(prl)) feature_wrist = FeaturePosition ('wrist_feature', robot.dynamic.arm_joint_5, robot.dynamic.Jarm_joint_5, goal) #feature_wrist.selec.value = 11100 task_wrist = Task ('wrist_task') task_wrist.controlGain.value = 1 task_wrist.add (feature_wrist.name) #feature_wrist.selec.value = '111' robot.addTrace (task_wrist.name, "error") #solver = SOT ('solver') solver.setSize (dimension) solver.push (task_waist.name) solver.push (task_wrist.name) robot.startTracer() plug (solver.control,robot. device.control) robot.stopTracer()