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()