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)
