def createTasks(self):
self.taskGaze = MetaTaskVisualPoint('gaze',self.robot.dynamic,'head','gaze')
self.contactRF = self.robot.contactRF
self.contactLF = self.robot.contactLF
self.taskCom = self.robot.mTasks['com']
self.taskLim = self.robot.taskLim
self.taskRH = self.robot.mTasks['rh']
self.taskLH = self.robot.mTasks['lh']
self.taskChest = self.robot.mTasks['chest']
self.taskPosture = self.robot.mTasks['posture']
self.taskGripper = MetaTaskKinePosture(self.robot.dynamic,'gripper')
self.taskHalfStitting = MetaTaskKinePosture(self.robot.dynamic,'halfsitting')
self.taskCompensate = MetaTaskKine6d('compensate',self.robot.dynamic,'rh','right-wrist')
def createTasks(self,robot):
self.taskHalfSitting = MetaTaskKinePosture(self.robot.dynamic,'halfsitting')
self.taskInitialPose = MetaTaskKinePosture(self.robot.dynamic,'initial-pose')
self.taskGripper = MetaTaskKinePosture(self.robot.dynamic,'gripper')
(self.tasks['trunk'],self.gains['trunk'])= self.createTrunkTask(robot, 'trunk')
self.taskRF = self.tasks['left-ankle']
self.taskLF = self.tasks['right-ankle']
self.taskCom = self.tasks['com']
self.taskRH = self.tasks['right-wrist']
self.taskLH = self.tasks['left-wrist']
self.taskTrunk = self.tasks['trunk']
self.taskPosture = self.tasks['posture']
self.taskBalance = self.tasks['balance']
self.taskWaist = self.tasks['waist']
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(self, robot):
self.taskHalfSitting = MetaTaskKinePosture(self.robot.dynamic,
'halfsitting')
self.taskInitialPose = MetaTaskKinePosture(self.robot.dynamic,
'initial-pose')
self.taskGripper = MetaTaskKinePosture(self.robot.dynamic, 'gripper')
(self.tasks['trunk'],
self.gains['trunk']) = self.createTrunkTask(robot, 'trunk')
self.taskRF = self.tasks['left-ankle']
self.taskLF = self.tasks['right-ankle']
self.taskCom = self.tasks['com']
self.taskRH = self.tasks['right-wrist']
self.taskLH = self.tasks['left-wrist']
self.taskTrunk = self.tasks['trunk']
self.taskPosture = self.tasks['posture']
self.taskBalance = self.tasks['balance']
self.taskWaist = self.tasks['waist']
taskElbow.task.controlGain.value = 0.9
# --- TASK SUPPORT --------------------------------------------------
featureSupport = FeatureGeneric('featureSupport')
plug(dyn.com,featureSupport.errorIN)
plug(dyn.Jcom,featureSupport.jacobianIN)
taskSupport=TaskInequality('taskSupport')
taskSupport.add(featureSupport.name)
taskSupport.selec.value = '011'
taskSupport.referenceInf.value = (-0.08,-0.15,0) # Xmin, Ymin
taskSupport.referenceSup.value = (0.11,0.15,0) # Xmax, Ymax
taskSupport.dt.value=dt
# --- POSTURE ---
taskPosture = MetaTaskKinePosture(dyn)
# --- CONTACTS
# define contactLF and contactRF
for name,joint in [ ['LF','left-ankle'], ['RF','right-ankle' ] ]:
contact = MetaTaskKine6d('contact'+name,dyn,name,joint)
contact.feature.frame('desired')
contact.gain.setConstant(10)
locals()['contact'+name] = contact
# --- TRACER -----------------------------------------------------------------
from dynamic_graph.tracer import *
tr = Tracer('tr')
tr.open('/tmp/','','.dat')
tr.start()
robot.after.addSignal('tr.triger')
# --- TASK SUPPORT SMALL --------------------------------------------
featureSupportSmall = FeatureGeneric('featureSupportSmall')
plug(dyn.com, featureSupportSmall.errorIN)
plug(dyn.Jcom, featureSupportSmall.jacobianIN)
taskSupportSmall = TaskInequality('taskSupportSmall')
taskSupportSmall.add(featureSupportSmall.name)
taskSupportSmall.selec.value = '011'
#taskSupportSmall.referenceInf.value = (-0.02,-0.02,0) # Xmin, Ymin
#askSupportSmall.referenceSup.value = (0.02,0.02,0) # Xmax, Ymax
taskSupportSmall.referenceInf.value = (-0.02, -0.05, 0) # Xmin, Ymin
taskSupportSmall.referenceSup.value = (0.02, 0.05, 0) # Xmax, Ymax
taskSupportSmall.dt.value = dt
# --- POSTURE ---
taskPosture = MetaTaskKinePosture(dyn)
# --- GAZE ---
taskGaze = MetaTaskVisualPoint('gaze', dyn, 'head', 'gaze')
# Head to camera matrix transform
# Camera RU headMcam=array([[0.0,0.0,1.0,0.0825],[1.,0.0,0.0,-0.029],[0.0,1.,0.0,0.102],[0.0,0.0,0.0,1.0]])
# Camera LL
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))
taskGaze.opmodif = matrixToTuple(headMcam)
# --- FOV ---
taskFoV = MetaTaskVisualPoint('FoV', dyn, 'head', 'gaze')
taskFoV.opmodif = matrixToTuple(headMcam)
class Hrp2Bike(Application):
tracesRealTime = True
initialPose = (
# Free flyer
0.,
0.,
0.,
0.,
0.,
0.,
# Legs
0.,
0.,
0.,
0.,
0.,
0.,
0.,
0.,
0.,
0.,
0.,
0.,
# Chest and head
0.,
0.,
0.,
0.,
# Arms
0.,
0.,
0.,
0.,
0.,
0.,
0.1,
0.,
0.,
0.,
0.,
0.,
0.,
0.1,
)
def __init__(self, robot, hands=True): #, forceSeqplay=True):
Application.__init__(self, robot)
self.sot = self.solver.sot
self.hands = hands
# self.robot=robot
# self.seq=Seqplay('seqplay')
self.createTasks(robot)
self.initTasks()
self.initTaskGains()
self.initSolver()
self.initialStack()
def printSolver(self): #show tasks in solver controling the robot
print self.solver
#----------TASKS-------------------------------
#------------------CREATION--------------------
def createTrunkTask(self, robot, taskName, ingain=1.0):
task = Task(taskName)
task.add(self.features['chest'].name)
task.add(self.features['waist'].name)
task.add(self.features['gaze'].name)
gain = GainAdaptive(taskName + 'gain')
gain.setConstant(ingain)
plug(gain.gain, task.controlGain)
plug(task.error, gain.error)
return (task, gain)
def createTasks(self, robot):
self.taskHalfSitting = MetaTaskKinePosture(self.robot.dynamic,
'halfsitting')
self.taskInitialPose = MetaTaskKinePosture(self.robot.dynamic,
'initial-pose')
self.taskGripper = MetaTaskKinePosture(self.robot.dynamic, 'gripper')
(self.tasks['trunk'],
self.gains['trunk']) = self.createTrunkTask(robot, 'trunk')
self.taskRF = self.tasks['left-ankle']
self.taskLF = self.tasks['right-ankle']
self.taskCom = self.tasks['com']
self.taskRH = self.tasks['right-wrist']
self.taskLH = self.tasks['left-wrist']
self.taskTrunk = self.tasks['trunk']
self.taskPosture = self.tasks['posture']
self.taskBalance = self.tasks['balance']
self.taskWaist = self.tasks['waist']
def openGripper(self):
self.taskGripper.gotoq(None,
rhand=(self.gripperOpen, ),
lhand=(self.gripperOpen, ))
def closeGripper(self):
self.taskGripper.gotoq(None,
rhand=(self.gripperClose, ),
lhand=(self.gripperClose, ))
#------------------INIT-TASK------------------
def initTasks(self):
self.initTaskBalance()
self.initTaskPosture()
if self.hands:
self.initTaskGripper()
self.initTaskHalfSitting()
self.initTaskInitialPose()
def initTaskBalance(self):
# --- BALANCE ---
self.features['chest'].frame('desired')
self.features['waist'].frame('desired')
self.features['chest'].selec.value = '111000'
self.features['waist'].selec.value = '111000'
self.featureCom.selec.value = '111'
def initTaskHalfSitting(self):
self.taskHalfSitting.gotoq(None,\
rleg =(self.robot.halfSitting[6:12]), \
lleg =(self.robot.halfSitting[12:18]), \
chest=(self.robot.halfSitting[18:20]), \
head =(self.robot.halfSitting[20:22]), \
rarm =(self.robot.halfSitting[22:28]), \
larm =(self.robot.halfSitting[29:35]))
def initTaskInitialPose(self):
self.taskInitialPose.gotoq(None,\
rleg =(self.initialPose[6:12]), \
lleg =(self.initialPose[12:18]), \
chest=(self.initialPose[18:20]), \
head =(self.initialPose[20:22]), \
rarm =(self.initialPose[22:28]), \
larm =(self.initialPose[29:35]))
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 initTaskGripper(self):
self.gripperOpen = degToRad(30)
self.gripperClose = degToRad(3)
self.openGripper()
#------------------INIT-GAIN------------------
def initTaskGains(self):
self.taskHalfSitting.gain.setByPoint(2, 0.2, 0.01, 0.8)
self.taskGripper.gain.setConstant(3)
self.taskInitialPose.gain.setByPoint(2, 0.2, 0.01, 0.8)
#----------SOLVER------------------------------
def initSolver(self):
plug(self.sot.control, self.robot.device.control)
def push(self, task): #push task in solver
if isinstance(task, str): taskName = task
elif "task" in task.__dict__: taskName = task.task.name
else: taskName = task.name
if taskName not in toList(self.sot):
self.sot.push(taskName)
if taskName != "posture" and "posture" in toList(self.sot):
self.sot.down("posture")
def removeTasks(self, task): #remove task from solver
if isinstance(task, str): taskName = task
elif "task" in task.__dict__: taskName = task.task.name
else: taskName = task.name
if taskName in toList(self.sot):
self.sot.remove(taskName)
def printSolver(self): #show tasks in solver controlling the robot
print self.solver
def showTasks(self): #show library of precomputed tasks
self.tasks
# --- TRACES -----------------------------------------------------------
def withTraces(self):
if self.tracesRealTime:
self.robot.initializeTracer()
else:
self.robot.tracer = Tracer('trace')
self.robot.device.after.addSignal('{0}.triger'.format(
self.robot.tracer.name))
self.robot.tracer.open('/tmp/', '', '.dat')
self.robot.startTracer()
def stopTraces(self):
self.robot.stopTracer()
def trace(self):
self.robot.tracer.dump()
#----------RUN---------------------------------
def goInitialPose(self):
self.sot.clear()
# self.push(self.taskBalance)
self.push(self.taskLF)
self.push(self.taskRF)
self.push(self.taskPosture)
if self.hands:
self.push(self.taskGripper)
self.push(self.taskInitialPose)
def initialStack(self):
self.push(self.taskBalance)
self.push(self.taskTrunk)
self.push(self.taskPosture)
if self.hands:
self.push(self.taskGripper)
def goHalfSitting(self):
self.sot.clear()
self.push(self.taskBalance)
self.push(self.taskPosture)
if self.hands:
self.push(self.taskGripper)
self.push(self.taskHalfSitting)
#------------parameters of bike-------
xg = 0.0
zg = 0.29 #center of crank gear
R = 0.17 #pedal-center of crank gear
def goBikeSitting(self):
self.sot.clear()
# self.push(self.taskBalance)
# self.push(self.tasks['chest'])
change6dPositionReference(self.taskWaist,self.features['waist'],\
self.gains['waist'],\
(-0.22,0.0,0.58,0,0,0),'111111')
self.push(self.taskWaist)
if self.hands:
self.push(self.taskGripper)
change6dPositionReference(self.taskRH,self.features['right-wrist'],\
self.gains['right-wrist'],\
(0.25,-0.255,0.90,-pi/2,0,pi/2),'111111')
self.push(self.taskRH)
change6dPositionReference(self.taskLH,self.features['left-wrist'],\
self.gains['left-wrist'],\
(0.25,0.255,0.90,pi/2,0,-pi/2),'111111')
self.push(self.taskLH)
change6dPositionReference(self.taskRF,self.features['right-ankle'],\
self.gains['right-ankle'],\
(0.0,-0.1125,0.12,0,0,degToRad(-8)),'111111')
self.push(self.taskRF)
change6dPositionReference(self.taskLF,self.features['left-ankle'],\
self.gains['left-ankle'],\
(0.0,0.1125,0.46,0,0,degToRad(8)),'111111')
self.push(self.taskLF)
self.push(self.taskPosture)
def stopMove(self):
self.rotation = False
def circle(
self,
nbPoint=16,
rotation=True): #nbPoint=number of point to dicretise the circle
self.rotation = rotation
self.stepCircle = (2 * pi) / nbPoint
circleL = []
circleR = []
for j in range(0, nbPoint):
circleL.append(-(j * self.stepCircle) + (pi / 2))
circleR.append(-(j * self.stepCircle) + (3 * pi / 2))
self.sot.clear()
change6dPositionReference(self.taskWaist,self.features['waist'],\
self.gains['waist'],\
(-0.22,0.0,0.58,0,0,0),'111111')
self.push(self.taskWaist)
if self.hands:
self.push(self.taskGripper)
change6dPositionReference(self.taskRH,self.features['right-wrist'],\
self.gains['right-wrist'],\
(0.25,-0.255,0.90,-pi/2,0,pi/2),'111111')
self.push(self.taskRH)
change6dPositionReference(self.taskLH,self.features['left-wrist'],\
self.gains['left-wrist'],\
(0.25,0.255,0.90,pi/2,0,-pi/2),'111111')
self.push(self.taskLH)
#--------rotation--------
self.push(self.taskRF)
self.push(self.taskLF)
self.push(self.taskPosture)
#while self.rotation:
for i in range(0, nbPoint):
xpL = (cos(circleL[i]) * self.R) + self.xg
zpL = (sin(circleL[i]) * self.R) + self.zg
xpR = (cos(circleR[i]) * self.R) + self.xg
zpR = (sin(circleR[i]) * self.R) + self.zg
change6dPositionReference(self.taskRF,self.features['right-ankle'],\
self.gains['right-ankle'],\
(xpR,-0.1125,zpR,0,0,degToRad(-8)),'111111')
change6dPositionReference(self.taskLF,self.features['left-ankle'],\
self.gains['left-ankle'],\
(xpL,0.1125,zpL,0,0,degToRad(8)),'111111')
time.sleep(5)
# --- SEQUENCER ---
step = 1
def sequencer(self, stepSeq=None):
if stepSeq != None:
self.step = stepSeq
#-----initial position------
if self.step == 0:
print "Step : ", self.step
self.goInitialPose()
print('Initial Pose')
self.step += 1
elif self.step == 1:
print "Step : ", self.step
self.goBikeSitting()
print('Bike Sitting')
self.step += 1
elif self.step == 2:
print "Step : ", self.step
if self.hands:
self.closeGripper()
print('Close Gripper')
self.step += 1
#-----move start------
elif self.step == 3:
print "Step : ", self.step
self.circle()
print('Start movement')
self.step += 1
#-----end of move-----
#elif self.step==4:
#print "Step : ", self.step
#self.stopMove()
#print('Stop movement')
# self.step+=1
elif self.step == 4: #5:
print "Step : ", self.step
if self.hands:
self.openGripper()
print('Open Gripper')
self.step += 1
else:
print "Step : ", self.step
self.goHalfSitting()
print('Half-Sitting')
self.step += 1
def __call__(self):
self.sequencer()
def __repr__(self):
self.sequencer()
return str()
taskElbow.task.controlGain.value = 0.9
# --- TASK SUPPORT --------------------------------------------------
featureSupport = FeatureGeneric('featureSupport')
plug(dyn.com, featureSupport.errorIN)
plug(dyn.Jcom, featureSupport.jacobianIN)
taskSupport = TaskInequality('taskSupport')
taskSupport.add(featureSupport.name)
taskSupport.selec.value = '011'
taskSupport.referenceInf.value = (-0.08, -0.15, 0) # Xmin, Ymin
taskSupport.referenceSup.value = (0.11, 0.15, 0) # Xmax, Ymax
taskSupport.dt.value = dt
# --- POSTURE ---
taskPosture = MetaTaskKinePosture(dyn)
# --- CONTACTS
# define contactLF and contactRF
for name, joint in [['LF', 'left-ankle'], ['RF', 'right-ankle']]:
contact = MetaTaskKine6d('contact' + name, dyn, name, joint)
contact.feature.frame('desired')
contact.gain.setConstant(10)
locals()['contact' + name] = contact
# --- TRACER -----------------------------------------------------------------
from dynamic_graph.tracer import *
tr = Tracer('tr')
tr.open('/tmp/', '', '.dat')
tr.start()
robot.after.addSignal('tr.triger')
class CompensaterApplication:
threePhaseScrew = True
tracesRealTime = False
def __init__(self,robot):
self.robot = robot
plug(self.robot.dynamic.com,self.robot.device.zmp)
self.initGeneralApplication()
self.sot = self.solver.sot
self.createTasks()
self.initTasks()
self.initTaskGains()
self.initSolver()
self.initialStack()
def initGeneralApplication(self):
from dynamic_graph.sot.application.velocity.precomputed_meta_tasks import initialize
self.solver = initialize(self.robot)
# --- TASKS --------------------------------------------------------------------
# --- TASKS --------------------------------------------------------------------
# --- TASKS --------------------------------------------------------------------
def createTasks(self):
self.taskGaze = MetaTaskVisualPoint('gaze',self.robot.dynamic,'head','gaze')
self.contactRF = self.robot.contactRF
self.contactLF = self.robot.contactLF
self.taskCom = self.robot.mTasks['com']
self.taskLim = self.robot.taskLim
self.taskRH = self.robot.mTasks['rh']
self.taskLH = self.robot.mTasks['lh']
self.taskChest = self.robot.mTasks['chest']
self.taskPosture = self.robot.mTasks['posture']
self.taskGripper = MetaTaskKinePosture(self.robot.dynamic,'gripper')
self.taskHalfStitting = MetaTaskKinePosture(self.robot.dynamic,'halfsitting')
self.taskCompensate = MetaTaskKine6d('compensate',self.robot.dynamic,'rh','right-wrist')
def openGripper(self):
self.taskGripper.gotoq(None,rhand=(self.gripperOpen,),lhand=(self.gripperOpen,))
def closeGripper(self):
self.taskGripper.gotoq(None,rhand=(self.gripperClose,),lhand=(self.gripperClose,))
#initialization is separated from the creation of the tasks because if we want to switch
#to second order controlm the initialization will remain while the creation is
#changed
def initTasks(self):
self.initTaskContact()
self.initTaskBalance()
self.initTaskPosture()
self.initTaskHalfSitting()
self.initTaskCompensate()
def initTaskHalfSitting(self):
self.taskHalfStitting.gotoq(None,self.robot.halfSitting)
def initTaskContact(self):
# --- CONTACTS
self.contactRF.feature.frame('desired')
self.contactLF.feature.frame('desired')
def initTaskBalance(self):
# --- BALANCE ---
self.taskChest.feature.frame('desired')
self.taskChest.feature.selec.value = '011000'
ljl = matrix(self.robot.dynamic.lowerJl.value).T
ujl = matrix(self.robot.dynamic.upperJl.value).T
ljl[9] = 0.65
ljl[15] = 0.65
self.taskLim.referenceInf.value = vectorToTuple(ljl)
self.taskLim.referenceSup.value = vectorToTuple(ujl)
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 initTaskGains(self, setup = "medium"):
if setup == "medium":
self.contactRF.gain.setConstant(10)
self.contactLF.gain.setConstant(10)
self.taskChest.gain.setConstant(10)
self.taskRH.gain.setByPoint(2,0.8,0.01,0.8)
self.taskCompensate.gain.setByPoint(2,0.8,0.01,0.8)
self.taskHalfStitting.gain.setByPoint(2,0.8,0.01,0.8)
# --- SOLVER ----------------------------------------------------------------
def initSolver(self):
plug(self.sot.control,self.robot.device.control)
def push(self,task,keep=False):
if isinstance(task,str): taskName=task
elif "task" in task.__dict__: taskName=task.task.name
else: taskName=task.name
if taskName not in self.sot.toList():
self.sot.push(taskName)
if taskName!="taskposture" and "taskposture" in self.sot.toList():
self.sot.down("taskposture")
if keep: task.keep()
def rm(self,task):
if isinstance(task,str): taskName=task
elif "task" in task.__dict__: taskName=task.task.name
else: taskName=task.name
if taskName in self.sot.toList(): self.sot.rm(taskName)
# --- DISPLAY -------------------------------------------------------------
def initDisplay(self):
self.robot.device.viewer.updateElementConfig('red',[0,0,-1,0,0,0])
self.robot.device.viewer.updateElementConfig('yellow',[0,0,-1,0,0,0])
def updateDisplay(self):
'''Display the various objects corresponding to the calcul graph. '''
None
# --- TRACES -----------------------------------------------------------
def withTraces(self):
if self.tracesRealTime:
self.robot.initializeTracer()
else:
self.robot.tracer = Tracer('trace')
self.robot.device.after.addSignal('{0}.triger'.format(self.robot.tracer.name))
self.robot.tracer.open('/tmp/','','.dat')
self.robot.tracer.add( self.taskRH.task.name+'.error','erh' )
self.robot.startTracer()
def stopTraces(self):
self.robot.stopTracer()
def trace(self):
self.robot.tracer.dump()
# --- RUN --------------------------------------------------------------
def initialStack(self):
self.sot.clear()
self.push(self.contactLF,True)
self.push(self.contactRF,True)
self.push(self.taskLim)
self.push(self.taskCom)
self.push(self.taskChest,True)
self.push(self.taskPosture)
def moveToInit(self):
'''Go to initial pose.'''
gotoNd(self.taskRH,(0.3,-0.2,1.1,0,-pi/2,0),'111001')
self.push(self.taskRH)
None
def goHalfSitting(self):
'''End of application, go to final pose.'''
self.sot.clear()
self.push(self.contactLF,True)
self.push(self.contactRF,True)
self.push(self.taskLim)
self.push(self.taskHalfStitting)
# --- SEQUENCER ---
seqstep = 0
def nextStep(self,step=None):
if step!=None: self.seqstep = step
if self.seqstep==0:
self.moveToInit()
elif self.seqstep==1:
self.startCompensate()
elif self.seqstep==2:
self.goHalfSitting()
self.seqstep += 1
def __add__(self,i):
self.nextStep()
# COMPENATION ######################################
def initTaskCompensate(self):
# w is the world, c is the egocenter (the central reference point of
# the robot kinematics).
# The constraint is:
# wMh0 !!=!! wMh = wMa0 a0Ma aMc cMh
# or written in cMh
# cMh !!=!! cMa aMa0 a0Mh0
# c : central frame of the robot
# cc : central frame for the controller (without the flexibility)
# ccMc= flexibility
self.ccMc = PoseUThetaToMatrixHomo('ccMc')
self.ccMc.sin.value = [0,]*6
# href : the reference 'desired' position of the hand
self.ccMhref = Multiply_of_matrixHomo('ccMhref')
plug(self.ccMc.sout,self.ccMhref.sin1)
# You need to set up a reference value here: plug( -- ,self.ccMhref.sin2)
plug(self.ccMhref.sout,self.taskCompensate.featureDes.position)
self.taskCompensate.feature.frame('desired')
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)
taskSupport.dt.value=dt
# --- TASK SUPPORT SMALL --------------------------------------------
featureSupportSmall = FeatureGeneric('featureSupportSmall')
plug(robot.dynamic.com,featureSupportSmall.errorIN)
plug(robot.dynamic.Jcom,featureSupportSmall.jacobianIN)
taskSupportSmall=TaskInequality('taskSupportSmall')
taskSupportSmall.add(featureSupportSmall.name)
taskSupportSmall.selec.value = '011'
taskSupportSmall.referenceInf.value = (-0.02,-0.05,0) # Xmin, Ymin
taskSupportSmall.referenceSup.value = (0.02,0.05,0) # Xmax, Ymax
taskSupportSmall.dt.value=dt
# --- POSTURE ---
taskPosture = MetaTaskKinePosture(robot.dynamic)
# --- GAZE ---
taskGaze = MetaTaskVisualPoint('gaze',robot.dynamic,'head','gaze')
# Head to camera matrix transform
# Camera RU headMcam=array([[0.0,0.0,1.0,0.0825],[1.,0.0,0.0,-0.029],[0.0,1.,0.0,0.102],[0.0,0.0,0.0,1.0]])
# Camera LL
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))
taskGaze.opmodif = matrixToTuple(headMcam)
# --- FOV ---
taskFoV = MetaTaskVisualPoint('FoV',robot.dynamic,'head','gaze')
taskFoV.opmodif = matrixToTuple(headMcam)
taskFoV.task=TaskInequality('taskFoVineq')
class Hrp2Bike(Application):
tracesRealTime = True
initialPose = (
# Free flyer
0., 0., 0., 0., 0. , 0.,
# Legs
0., 0., 0., 0., 0., 0.,
0., 0., 0., 0., 0., 0.,
# Chest and head
0., 0., 0., 0.,
# Arms
0., 0., 0., 0., 0., 0., 0.1,
0., 0., 0., 0., 0., 0., 0.1,
)
def __init__(self,robot,hands=True):#, forceSeqplay=True):
Application.__init__(self,robot)
self.sot=self.solver.sot
self.hands=hands
# self.robot=robot
# self.seq=Seqplay('seqplay')
self.createTasks(robot)
self.initTasks()
self.initTaskGains()
self.initSolver()
self.initialStack()
def printSolver(self): #show tasks in solver controling the robot
print self.solver
#----------TASKS-------------------------------
#------------------CREATION--------------------
def createTrunkTask (self, robot, taskName, ingain=1.0):
task = Task (taskName)
task.add (self.features['chest'].name)
task.add (self.features['waist'].name)
task.add (self.features['gaze'].name)
gain = GainAdaptive(taskName+'gain')
gain.setConstant(ingain)
plug(gain.gain, task.controlGain)
plug(task.error, gain.error)
return (task, gain)
def createTasks(self,robot):
self.taskHalfSitting = MetaTaskKinePosture(self.robot.dynamic,'halfsitting')
self.taskInitialPose = MetaTaskKinePosture(self.robot.dynamic,'initial-pose')
self.taskGripper = MetaTaskKinePosture(self.robot.dynamic,'gripper')
(self.tasks['trunk'],self.gains['trunk'])= self.createTrunkTask(robot, 'trunk')
self.taskRF = self.tasks['left-ankle']
self.taskLF = self.tasks['right-ankle']
self.taskCom = self.tasks['com']
self.taskRH = self.tasks['right-wrist']
self.taskLH = self.tasks['left-wrist']
self.taskTrunk = self.tasks['trunk']
self.taskPosture = self.tasks['posture']
self.taskBalance = self.tasks['balance']
self.taskWaist = self.tasks['waist']
def openGripper(self):
self.taskGripper.gotoq(None,rhand=(self.gripperOpen,),lhand=(self.gripperOpen,))
def closeGripper(self):
self.taskGripper.gotoq(None,rhand=(self.gripperClose,),lhand=(self.gripperClose,))
#------------------INIT-TASK------------------
def initTasks(self):
self.initTaskBalance()
self.initTaskPosture()
if self.hands:
self.initTaskGripper()
self.initTaskHalfSitting()
self.initTaskInitialPose()
def initTaskBalance(self):
# --- BALANCE ---
self.features['chest'].frame('desired')
self.features['waist'].frame('desired')
self.features['chest'].selec.value = '111000'
self.features['waist'].selec.value = '111000'
self.featureCom.selec.value = '111'
def initTaskHalfSitting(self):
self.taskHalfSitting.gotoq(None,\
rleg =(self.robot.halfSitting[6:12]), \
lleg =(self.robot.halfSitting[12:18]), \
chest=(self.robot.halfSitting[18:20]), \
head =(self.robot.halfSitting[20:22]), \
rarm =(self.robot.halfSitting[22:28]), \
larm =(self.robot.halfSitting[29:35]))
def initTaskInitialPose(self):
self.taskInitialPose.gotoq(None,\
rleg =(self.initialPose[6:12]), \
lleg =(self.initialPose[12:18]), \
chest=(self.initialPose[18:20]), \
head =(self.initialPose[20:22]), \
rarm =(self.initialPose[22:28]), \
larm =(self.initialPose[29:35]))
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 initTaskGripper(self):
self.gripperOpen = degToRad(30)
self.gripperClose = degToRad(3)
self.openGripper()
#------------------INIT-GAIN------------------
def initTaskGains(self):
self.taskHalfSitting.gain.setByPoint(2,0.2,0.01,0.8)
self.taskGripper.gain.setConstant(3)
self.taskInitialPose.gain.setByPoint(2,0.2,0.01,0.8)
#----------SOLVER------------------------------
def initSolver(self):
plug(self.sot.control,self.robot.device.control)
def push(self,task): #push task in solver
if isinstance(task,str): taskName=task
elif "task" in task.__dict__: taskName=task.task.name
else: taskName=task.name
if taskName not in toList(self.sot):
self.sot.push(taskName)
if taskName!="posture" and "posture" in toList(self.sot):
self.sot.down("posture")
def removeTasks(self,task) : #remove task from solver
if isinstance(task,str): taskName=task
elif "task" in task.__dict__: taskName=task.task.name
else: taskName=task.name
if taskName in toList(self.sot):
self.sot.remove(taskName)
def printSolver(self): #show tasks in solver controlling the robot
print self.solver
def showTasks(self) : #show library of precomputed tasks
self.tasks
# --- TRACES -----------------------------------------------------------
def withTraces(self):
if self.tracesRealTime:
self.robot.initializeTracer()
else:
self.robot.tracer = Tracer('trace')
self.robot.device.after.addSignal('{0}.triger'.format(self.robot.tracer.name))
self.robot.tracer.open('/tmp/','','.dat')
self.robot.startTracer()
def stopTraces(self):
self.robot.stopTracer()
def trace(self):
self.robot.tracer.dump()
#----------RUN---------------------------------
def goInitialPose(self):
self.sot.clear()
# self.push(self.taskBalance)
self.push(self.taskLF)
self.push(self.taskRF)
self.push(self.taskPosture)
if self.hands:
self.push(self.taskGripper)
self.push(self.taskInitialPose)
def initialStack(self):
self.push(self.taskBalance)
self.push(self.taskTrunk)
self.push(self.taskPosture)
if self.hands:
self.push(self.taskGripper)
def goHalfSitting(self):
self.sot.clear()
self.push(self.taskBalance)
self.push(self.taskPosture)
if self.hands:
self.push(self.taskGripper)
self.push(self.taskHalfSitting)
#------------parameters of bike-------
xg=0.0; zg=0.29 #center of crank gear
R=0.17 #pedal-center of crank gear
def goBikeSitting(self):
self.sot.clear()
# self.push(self.taskBalance)
# self.push(self.tasks['chest'])
change6dPositionReference(self.taskWaist,self.features['waist'],\
self.gains['waist'],\
(-0.22,0.0,0.58,0,0,0),'111111')
self.push(self.taskWaist)
if self.hands:
self.push(self.taskGripper)
change6dPositionReference(self.taskRH,self.features['right-wrist'],\
self.gains['right-wrist'],\
(0.25,-0.255,0.90,-pi/2,0,pi/2),'111111')
self.push(self.taskRH)
change6dPositionReference(self.taskLH,self.features['left-wrist'],\
self.gains['left-wrist'],\
(0.25,0.255,0.90,pi/2,0,-pi/2),'111111')
self.push(self.taskLH)
change6dPositionReference(self.taskRF,self.features['right-ankle'],\
self.gains['right-ankle'],\
(0.0,-0.1125,0.12,0,0,degToRad(-8)),'111111')
self.push(self.taskRF)
change6dPositionReference(self.taskLF,self.features['left-ankle'],\
self.gains['left-ankle'],\
(0.0,0.1125,0.46,0,0,degToRad(8)),'111111')
self.push(self.taskLF)
self.push(self.taskPosture)
def stopMove(self):
self.rotation=False
def circle(self,nbPoint=16,rotation=True): #nbPoint=number of point to dicretise the circle
self.rotation=rotation
self.stepCircle=(2*pi)/nbPoint
circleL=[]
circleR=[]
for j in range(0,nbPoint):
circleL.append(-(j*self.stepCircle)+(pi/2))
circleR.append(-(j*self.stepCircle)+(3*pi/2))
self.sot.clear()
change6dPositionReference(self.taskWaist,self.features['waist'],\
self.gains['waist'],\
(-0.22,0.0,0.58,0,0,0),'111111')
self.push(self.taskWaist)
if self.hands:
self.push(self.taskGripper)
change6dPositionReference(self.taskRH,self.features['right-wrist'],\
self.gains['right-wrist'],\
(0.25,-0.255,0.90,-pi/2,0,pi/2),'111111')
self.push(self.taskRH)
change6dPositionReference(self.taskLH,self.features['left-wrist'],\
self.gains['left-wrist'],\
(0.25,0.255,0.90,pi/2,0,-pi/2),'111111')
self.push(self.taskLH)
#--------rotation--------
self.push(self.taskRF)
self.push(self.taskLF)
self.push(self.taskPosture)
#while self.rotation:
for i in range(0,nbPoint):
xpL=(cos(circleL[i])*self.R)+self.xg
zpL=(sin(circleL[i])*self.R)+self.zg
xpR=(cos(circleR[i])*self.R)+self.xg
zpR=(sin(circleR[i])*self.R)+self.zg
change6dPositionReference(self.taskRF,self.features['right-ankle'],\
self.gains['right-ankle'],\
(xpR,-0.1125,zpR,0,0,degToRad(-8)),'111111')
change6dPositionReference(self.taskLF,self.features['left-ankle'],\
self.gains['left-ankle'],\
(xpL,0.1125,zpL,0,0,degToRad(8)),'111111')
time.sleep(5)
# --- SEQUENCER ---
step=1
def sequencer(self,stepSeq=None):
if stepSeq!=None:
self.step=stepSeq
#-----initial position------
if self.step==0:
print "Step : ", self.step
self.goInitialPose()
print('Initial Pose')
self.step+=1
elif self.step==1:
print "Step : ", self.step
self.goBikeSitting()
print('Bike Sitting')
self.step+=1
elif self.step==2:
print "Step : ", self.step
if self.hands:
self.closeGripper()
print('Close Gripper')
self.step+=1
#-----move start------
elif self.step==3:
print "Step : ", self.step
self.circle()
print('Start movement')
self.step+=1
#-----end of move-----
#elif self.step==4:
#print "Step : ", self.step
#self.stopMove()
#print('Stop movement')
# self.step+=1
elif self.step==4:#5:
print "Step : ", self.step
if self.hands:
self.openGripper()
print('Open Gripper')
self.step+=1
else:
print "Step : ", self.step
self.goHalfSitting()
print('Half-Sitting')
self.step+=1
def __call__(self):
self.sequencer()
def __repr__(self):
self.sequencer()
return str()
# ---- TASKS -------------------------------------------------------------------
# ---- TASKS -------------------------------------------------------------------
# ---- TASKS -------------------------------------------------------------------
# ---- TASK GRIP ---
taskRH=MetaTaskKine6d('rh',dyn,'rh','right-wrist')
handMgrip=eye(4); handMgrip[0:3,3] = (0,0,-0.17)
taskRH.opmodif = matrixToTuple(handMgrip)
# --- STATIC COM (if not walking)
taskCom0 = MetaTaskKineCom(dyn)
# --- POSTURE ---
taskPosture = MetaTaskKinePosture(dyn)
# --- GAZE ---
taskGaze = MetaTaskKine6d('gaze',dyn,'head','gaze')
# Head to camera matrix transform
headMcam=array([[0.0,0.0,1.0,0.0825],[1.,0.0,0.0,-0.029],[0.0,1.,0.0,0.102],[0.0,0.0,0.0,1.0]])
taskGaze.opmodif = matrixToTuple(headMcam)
taskGaze.feature.frame('current')
taskGaze.feature.selec.value = '011'
# --- Task Joint Limits -----------------------------------------
dyn.upperJl.recompute(0)
dyn.lowerJl.recompute(0)
taskJL = TaskJointLimits('taskJL')
plug(dyn.position,taskJL.position)
taskJL.controlGain.value = 10
class HandCompensaterKine:
threePhaseScrew = True
tracesRealTime = True
def __init__(self,robot,twoHands=True):
self.robot = robot
self.twoHands = twoHands
plug(self.robot.dynamic.com,self.robot.device.zmp)
self.initGeneralApplication()
self.sot = self.solver.sot
self.createTasks()
self.initTasks()
self.initTaskGains()
self.initSolver()
self.initialStack()
def initGeneralApplication(self):
self.solver = initialize(self.robot)
# --- TASKS --------------------------------------------------------------------
# --- TASKS --------------------------------------------------------------------
# --- TASKS --------------------------------------------------------------------
def createTasks(self):
self.taskGaze = MetaTaskVisualPoint('gaze',self.robot.dynamic,'head','gaze')
self.contactRF = self.robot.contactRF
self.contactLF = self.robot.contactLF
self.taskCom = self.robot.mTasks['com']
self.taskLim = self.robot.taskLim
self.taskRH = self.robot.mTasks['rh']
if self.twoHands:
self.taskLH = self.robot.mTasks['lh']
self.taskChest = self.robot.mTasks['chest']
self.taskPosture = self.robot.mTasks['posture']
self.taskGripper = MetaTaskKinePosture(self.robot.dynamic,'gripper')
self.taskHalfStitting = MetaTaskKinePosture(self.robot.dynamic,'halfsitting')
self.taskCompensateR = MetaTaskKine6d('compensateR',self.robot.dynamic,'rh','right-wrist')
if self.twoHands:
self.taskCompensateL = MetaTaskKine6d('compensateL',self.robot.dynamic,'lh','left-wrist')
def openGripper(self):
self.taskGripper.gotoq(None,rhand=(self.gripperOpen,),lhand=(self.gripperOpen,))
def closeGripper(self):
self.taskGripper.gotoq(None,rhand=(self.gripperClose,),lhand=(self.gripperClose,))
#initialization is separated from the creation of the tasks because if we want to switch
#to second order controlm the initialization will remain while the creation is
#changed
def initTasks(self):
self.initTaskContact()
self.initTaskBalance()
self.initTaskPosture()
self.initTaskHalfSitting()
self.initTaskCompensate()
def initTaskHalfSitting(self):
self.taskHalfStitting.gotoq(None,self.robot.halfSitting)
def initTaskContact(self):
# --- CONTACTS
self.contactRF.feature.frame('desired')
self.contactLF.feature.frame('desired')
def initTaskBalance(self):
# --- BALANCE ---
self.taskChest.feature.frame('desired')
#self.taskChest.feature.selec.value = '111111'
self.taskChest.feature.selec.value = '111000'
#self.taskCom.feature.selec.value = '111'
ljl = matrix(self.robot.dynamic.lowerJl.value).T
ujl = matrix(self.robot.dynamic.upperJl.value).T
ljl[9] = 0.65
ljl[15] = 0.65
self.taskLim.referenceInf.value = vectorToTuple(ljl)
self.taskLim.referenceSup.value = vectorToTuple(ujl)
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]
mask = '000000000000111100000000000000'
# robot.dynamic.displaySignals ()
# robot.dynamic.Jchest.value
self.taskPosture.feature.selec.value = mask
def initTaskGains(self, setup = "medium"):
if setup == "medium":
self.contactRF.gain.setConstant(10)
self.contactLF.gain.setConstant(10)
self.taskChest.gain.setConstant(10)
self.taskRH.gain.setByPoint(4,0.2,0.01,0.8)
self.taskCompensateR.gain.setByPoint(4,0.2,0.01,0.8)
if self.twoHands:
self.taskCompensateL.gain.setByPoint(4,0.2,0.01,0.8)
self.taskLH.gain.setByPoint(4,0.2,0.01,0.8)
#self.taskCompensate.gain.setConstant(2)
self.taskHalfStitting.gain.setByPoint(2,0.2,0.01,0.8)
# --- SOLVER ----------------------------------------------------------------
def initSolver(self):
plug(self.sot.control,self.robot.device.control)
def push(self,task,keep=False):
if isinstance(task,str): taskName=task
elif "task" in task.__dict__: taskName=task.task.name
else: taskName=task.name
if taskName not in self.sot.toList():
self.sot.push(taskName)
if taskName!="taskposture" and "taskposture" in self.sot.toList():
self.sot.down("taskposture")
if keep: task.keep()
def rm(self,task):
if isinstance(task,str): taskName=task
elif "task" in task.__dict__: taskName=task.task.name
else: taskName=task.name
if taskName in self.sot.toList(): self.sot.rm(taskName)
# --- DISPLAY -------------------------------------------------------------
def initDisplay(self):
self.robot.device.viewer.updateElementConfig('red',[0,0,-1,0,0,0])
self.robot.device.viewer.updateElementConfig('yellow',[0,0,-1,0,0,0])
def updateDisplay(self):
'''Display the various objects corresponding to the calcul graph. '''
None
# --- TRACES -----------------------------------------------------------
def withTraces(self):
if self.tracesRealTime:
self.robot.tracerSize = 2**26
self.robot.initializeTracer()
else:
self.robot.tracer = Tracer('trace')
self.robot.device.after.addSignal('{0}.triger'.format(self.robot.tracer.name))
self.robot.tracer.open('/tmp/','','.dat')
#self.robot.tracer.add( self.taskRH.task.name+'.error','erh' )
def stopTracer(self):
self.robot.stopTracer()
def dumpTracer(self):
self.robot.tracer.dump()
def startTracer(self):
self.robot.startTracer()
# --- RUN --------------------------------------------------------------
def initialStack(self):
self.sot.clear()
self.push(self.contactLF,True)
self.push(self.contactRF,True)
self.push(self.taskLim)
self.push(self.taskCom)
self.push(self.taskChest,True)
self.push(self.taskPosture)
def moveToInit(self):
'''Go to initial pose.'''
#gotoNd(self.taskRH,(0.3,-0.2,1.1,0,-pi/2,0),'111001')
#gotoNd(self.taskLH,(0.3,0.2,1.1,0,-pi/2,0),'111001')
gotoNd(self.taskRH,(0.3,-0.2,1.1,0,-pi/2,0),'111001')
self.push(self.taskRH)
if self.twoHands:
gotoNd(self.taskLH,(0.3,0.2,1.1,0,-pi/2,0),'111001')
self.push(self.taskLH)
None
def goHalfSitting(self):
'''End of application, go to final pose.'''
self.sot.clear()
self.push(self.contactLF,True)
self.push(self.contactRF,True)
self.push(self.taskLim)
self.push(self.taskHalfStitting)
# --- SEQUENCER ---
seqstep = 0
def nextStep(self,step=None):
if step!=None: self.seqstep = step
if self.seqstep==0:
self.moveToInit()
elif self.seqstep==1:
self.startCompensate()
elif self.seqstep==2:
self.goHalfSitting()
self.seqstep += 1
def __add__(self,i):
self.nextStep()
# COMPENATION ######################################
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 startCompensate(self):
'''Start to compensate for the hand movements.'''
self.cMrhref.value = self.robot.dynamic.rh.value
self.cVrhref.value = (0.0 , 0.0 , 0.0 , 0.0 , 0.0 , 0.0)
print matrix(self.cMrhref.value)
#######
#self.cMlhref.value = self.robot.dynamic.lh.value
#self.cVlhref.value = (0.0 , 0.0 , 0.0 , 0.0 , 0.0 , 0.0)
#print matrix(self.cMlhref.value)
######
self.rm(self.taskRH)
self.push(self.taskCompensateR)
if self.twoHands:
self.rm(self.taskLH)
self.push(self.taskCompensateL)
# ---- TASKS -------------------------------------------------------------------
# ---- TASKS -------------------------------------------------------------------
# ---- TASKS -------------------------------------------------------------------
# ---- TASK GRIP ---
taskRH = MetaTaskKine6d('rh', dyn, 'rh', 'right-wrist')
handMgrip = eye(4)
handMgrip[0:3, 3] = (0, 0, -0.17)
taskRH.opmodif = matrixToTuple(handMgrip)
# --- STATIC COM (if not walking)
taskCom0 = MetaTaskKineCom(dyn)
# --- POSTURE ---
taskPosture = MetaTaskKinePosture(dyn)
# --- GAZE ---
taskGaze = MetaTaskKine6d('gaze', dyn, 'head', 'gaze')
# Head to camera matrix transform
headMcam = array([[0.0, 0.0, 1.0, 0.0825], [1., 0.0, 0.0, -0.029],
[0.0, 1., 0.0, 0.102], [0.0, 0.0, 0.0, 1.0]])
taskGaze.opmodif = matrixToTuple(headMcam)
taskGaze.feature.frame('current')
taskGaze.feature.selec.value = '011'
# --- Task Joint Limits -----------------------------------------
dyn.upperJl.recompute(0)
dyn.lowerJl.recompute(0)
taskJL = TaskJointLimits('taskJL')
plug(dyn.position, taskJL.position)
# --- TASK SUPPORT SMALL --------------------------------------------
featureSupportSmall = FeatureGeneric('featureSupportSmall')
plug(dyn.com,featureSupportSmall.errorIN)
plug(dyn.Jcom,featureSupportSmall.jacobianIN)
taskSupportSmall=TaskInequality('taskSupportSmall')
taskSupportSmall.add(featureSupportSmall.name)
taskSupportSmall.selec.value = '011'
#taskSupportSmall.referenceInf.value = (-0.02,-0.02,0) # Xmin, Ymin
#askSupportSmall.referenceSup.value = (0.02,0.02,0) # Xmax, Ymax
taskSupportSmall.referenceInf.value = (-0.02,-0.05,0) # Xmin, Ymin
taskSupportSmall.referenceSup.value = (0.02,0.05,0) # Xmax, Ymax
taskSupportSmall.dt.value=dt
# --- POSTURE ---
taskPosture = MetaTaskKinePosture(dyn)
# --- GAZE ---
taskGaze = MetaTaskVisualPoint('gaze',dyn,'head','gaze')
# Head to camera matrix transform
# Camera RU headMcam=array([[0.0,0.0,1.0,0.0825],[1.,0.0,0.0,-0.029],[0.0,1.,0.0,0.102],[0.0,0.0,0.0,1.0]])
# Camera LL
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))
taskGaze.opmodif = matrixToTuple(headMcam)
# --- FOV ---
taskFoV = MetaTaskVisualPoint('FoV',dyn,'head','gaze')
taskFoV.opmodif = matrixToTuple(headMcam)
taskFoV.task=TaskInequality('taskFoVineq')
