class MetaTaskCom(object):
def __init__(self, dyn, name="com"):
self.dyn = dyn
self.name = name
# dyn.setProperty('ComputeCoM','true')
self.feature = FeatureGeneric('feature' + name)
self.featureDes = FeatureGeneric('featureDes' + name)
self.gain = GainAdaptive('gain' + name)
plug(dyn.com, self.feature.errorIN)
plug(dyn.Jcom, self.feature.jacobianIN)
self.feature.setReference(self.featureDes.name)
def plugTask(self):
self.task.add(self.feature.name)
plug(self.task.error, self.gain.error)
plug(self.gain.gain, self.task.controlGain)
@property
def ref(self):
return self.featureDes.errorIN.value
@ref.setter
def ref(self, v):
self.featureDes.errorIN.value = v
class MetaTaskConfig(object):
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 plugTask(self):
self.task.add(self.feature.name)
plug(self.task.error, self.gain.error)
plug(self.gain.gain, self.task.controlGain)
@property
def ref(self):
return self.featureDes.errorIN.value
@ref.setter
def ref(self, v):
self.featureDes.errorIN.value = v
class MetaTaskJoint(object):
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)
def plugTask(self):
self.task.add(self.feature.name)
plug(self.task.error, self.gain.error)
plug(self.gain.gain, self.task.controlGain)
@property
def ref(self):
return self.featureDes.errorIN.value
@ref.setter
def ref(self, v):
self.featureDes.errorIN.value = v
def __init__(self, dyn, name="com"):
self.dyn = dyn
self.name = name
# dyn.setProperty('ComputeCoM','true')
self.feature = FeatureGeneric('feature' + name)
self.featureDes = FeatureGeneric('featureDes' + name)
self.gain = GainAdaptive('gain' + name)
plug(dyn.com, self.feature.errorIN)
plug(dyn.Jcom, self.feature.jacobianIN)
self.feature.setReference(self.featureDes.name)
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)
class CompoundDrive (object):
def __init__(self, name, device):
self.featureGeneric = FeatureGeneric(name)
self.device = device
self.index1 = 6 + allJoints.index('RHipYawPitch')
self.index2 = 6 + allJoints.index('LHipYawPitch')
def update(self):
q = self.device.signal('state').value
i1 = self.index1
i2 = self.index2
self.featureGeneric.signal('errorIN').value = (q[i2]+q[i1],)
self.featureGeneric.signal('jacobianIN').value = \
(i1*(0.,) + (1.,) +
(i2-i1-1)*(0.,) + (1.,) +
(6+len(allJoints)-i2-1)*(0.,),)
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 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 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 createCenterOfMassFeatureAndTask(robot, featureName, featureDesName, taskName, selec='111', ingain=1.):
robot.dynamic.com.recompute(0)
robot.dynamic.Jcom.recompute(0)
featureCom = FeatureGeneric(featureName)
plug(robot.dynamic.com, featureCom.errorIN)
plug(robot.dynamic.Jcom, featureCom.jacobianIN)
featureCom.selec.value = selec
featureComDes = FeatureGeneric(featureDesName)
featureComDes.errorIN.value = robot.dynamic.com.value
featureCom.setReference(featureComDes.name)
taskCom = Task(taskName)
taskCom.add(featureName)
gainCom = GainAdaptive('gain' + taskName)
gainCom.setConstant(ingain)
plug(gainCom.gain, taskCom.controlGain)
plug(taskCom.error, gainCom.error)
return (featureCom, featureComDes, taskCom, gainCom)
def createCenterOfMassFeatureAndTask(self,
featureName, featureDesName,
taskName,
selec = '011',
gain = 1.):
self.dynamic.com.recompute(0)
self.dynamic.Jcom.recompute(0)
featureCom = FeatureGeneric(featureName)
plug(self.dynamic.com, featureCom.errorIN)
plug(self.dynamic.Jcom, featureCom.jacobianIN)
featureCom.selec.value = selec
featureComDes = FeatureGeneric(featureDesName)
featureComDes.errorIN.value = self.dynamic.com.value
featureCom.setReference(featureComDes.name)
comTask = Task(taskName)
comTask.add(featureName)
comTask.controlGain.value = gain
return (featureCom, featureComDes, comTask)
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)
def __init__ (self, name, sotrobot):
super(Posture, self).__init__()
from dynamic_graph.sot.core import Task, FeatureGeneric, GainAdaptive, Selec_of_vector
from dynamic_graph.sot.core.matrix_util import matrixToTuple
from dynamic_graph import plug
from numpy import identity, hstack, zeros
n = Posture.sep + name
self.tp = Task ('task' + n)
self.tp.dyn = sotrobot.dynamic
self.tp.feature = FeatureGeneric('feature_'+n)
self.tp.featureDes = FeatureGeneric('feature_des_'+n)
self.tp.gain = GainAdaptive("gain_"+n)
robotDim = sotrobot.dynamic.getDimension()
self.tp.feature.jacobianIN.value = matrixToTuple( identity(robotDim) )
self.tp.feature.setReference(self.tp.featureDes.name)
self.tp.add(self.tp.feature.name)
# Connects the dynamics to the current feature of the posture task
plug(sotrobot.dynamic.position, self.tp.feature.errorIN)
self.tp.setWithDerivative (True)
# Set the gain of the posture task
setGain(self.tp.gain,10)
plug(self.tp.gain.gain, self.tp.controlGain)
plug(self.tp.error, self.tp.gain.error)
self.tasks = [ self.tp ]
self.topics = {
name: {
"type": "vector",
"topic": "/hpp/target/position",
"signalGetters": [ self._signalPositionRef ] },
"vel_" + name: {
"type": "vector",
"topic": "/hpp/target/velocity",
"signalGetters": [ self._signalVelocityRef ] },
}
def createCenterOfMassFeatureAndTask(robot,
featureName,
featureDesName,
taskName,
selec='111',
ingain=1.):
robot.dynamic.com.recompute(0)
robot.dynamic.Jcom.recompute(0)
featureCom = FeatureGeneric(featureName)
plug(robot.dynamic.com, featureCom.errorIN)
plug(robot.dynamic.Jcom, featureCom.jacobianIN)
featureCom.selec.value = selec
featureComDes = FeatureGeneric(featureDesName)
featureComDes.errorIN.value = robot.dynamic.com.value
featureCom.setReference(featureComDes.name)
taskCom = Task(taskName)
taskCom.add(featureName)
gainCom = GainAdaptive('gain' + taskName)
gainCom.setConstant(ingain)
plug(gainCom.gain, taskCom.controlGain)
plug(taskCom.error, gainCom.error)
return (featureCom, featureComDes, taskCom, gainCom)
def setupPostureTask():
global postureTask, postureError, postureFeature
initialGain = 0.1
postureTask = Task(robot.name + '_posture')
postureFeature = FeatureGeneric(robot.name + '_postureFeature')
postureError = PostureError('PostureError')
posture = list(robot.halfSitting)
posture[6 + 17] -= 1
posture[6 + 24] += 1
# postureError.setPosture(tuple(posture))
plug(robot.device.state, postureError.state)
plug(postureError.error, postureFeature.errorIN)
postureFeature.jacobianIN.value = computeJacobian()
postureTask.add(postureFeature.name)
postureTask.controlGain.value = initialGain
#from dynamic_graph.sot.core.meta_task_posture import MetaTaskKinePosture
from dynamic_graph.sot.core.matrix_util import matrixToTuple
from dynamic_graph.sot.tools import SimpleSeqPlay
from numpy import eye
from dynamic_graph.sot.core import Task, FeatureGeneric, GainAdaptive
from dynamic_graph.sot.core.meta_tasks import setGain
from dynamic_graph.sot.core.matrix_util import matrixToTuple
from numpy import identity, hstack, zeros
# Create the posture task
task_name = "posture_task"
taskPosture = Task(task_name)
taskPosture.dyn = robot.dynamic
taskPosture.feature = FeatureGeneric('feature_' + task_name)
taskPosture.featureDes = FeatureGeneric('feature_des_' + task_name)
taskPosture.gain = GainAdaptive("gain_" + task_name)
robotDim = robot.dynamic.getDimension()
first_6 = zeros((32, 6))
other_dof = identity(robotDim - 6)
jacobian_posture = hstack([first_6, other_dof])
taskPosture.feature.jacobianIN.value = matrixToTuple(jacobian_posture)
taskPosture.feature.setReference(taskPosture.featureDes.name)
taskPosture.add(taskPosture.feature.name)
# Create the sequence player
aSimpleSeqPlay = SimpleSeqPlay('aSimpleSeqPlay')
aSimpleSeqPlay.load(
"/home/ostasse/devel-src/robotpkg-test3/install/share/pyrene-motions/identification/OEM_arms_60s_500Hz"
)
aSimpleSeqPlay.setTimeToStart(10.0)
def __init__(self, name, device):
self.featureGeneric = FeatureGeneric(name)
self.device = device
self.index1 = 6 + allJoints.index('RHipYawPitch')
self.index2 = 6 + allJoints.index('LHipYawPitch')
R3 = ((0., 0., -1.),
(0., 1., 0.),
(1., 0., 0.))
eye3.set(R3)
p6.signal('selec').value = '000111'
p6.frame('current')
# --- COM
dyn.setProperty('ComputeCoM', 'true')
# dyn.setProperty ComputeVelocity true
# dyn.setProperty ComputeMomentum true
# dyn.setProperty ComputeZMP true
featureCom = FeatureGeneric('featureCom')
plug(dyn.signal('com'), featureCom.signal('errorIN'))
plug(dyn.signal('Jcom'), featureCom.signal('jacobianIN'))
# set featureCom.selec 111
featureComDes = FeatureGeneric('featureComDes')
# set featureComDes.errorIN [2](0,-0)
featureCom.signal('sdes').value = featureComDes
taskCom = Task('taskCom')
taskCom.add('featureCom')
taskCom.signal('controlGain').value = .3
# set taskCom.controlGain 1
# --- CHEST
def __init__(self, robot, solver, trace = None, postureTaskDofs = None):
print("Constructor of LegsFollower Graph")
self.robot = robot
self.solver = solver
self.legsFollower = LegsFollower('legs-follower')
self.statelength = len(robot.device.state.value)
# Initialize the posture task.
print("Posture Task")
self.postureTaskDofs = postureTaskDofs
if not self.postureTaskDofs:
self.postureTaskDofs = []
for i in xrange(len(robot.halfSitting) - 6):
# Disable legs dofs.
if i < 12: #FIXME: not generic enough
self.postureTaskDofs.append((i + 6, False))
else:
self.postureTaskDofs.append((i + 6, True))
# This part is taken from feet_follower_graph
self.postureTask = Task(self.robot.name + '_posture')
self.postureFeature = FeaturePosture(self.robot.name + '_postureFeature')
plug(self.robot.device.state, self.postureFeature.state)
posture = list(self.robot.halfSitting)
self.postureFeature.setPosture(tuple(posture))
for (dof, isEnabled) in self.postureTaskDofs:
self.postureFeature.selectDof(dof, isEnabled)
self.postureTask.add(self.postureFeature.name)
self.postureTask.controlGain.value = 2.
# Initialize the waist follower task.
print("Waist Task")
self.robot.features['waist'].selec.value = '111111'
plug(self.legsFollower.waist, self.robot.features['waist'].reference)
self.robot.tasks['waist'].controlGain.value = 1.
# Initialize the legs follower task.
print("Legs Task")
self.legsTask = Task(self.robot.name + '_legs')
self.legsFeature = FeatureGeneric(self.robot.name + '_legsFeature')
legsFeatureDesName = self.robot.name + '_legsFeatureDes'
self.legsFeatureDes = FeatureGeneric(legsFeatureDesName)
self.legsError = LegsError('LegsError')
plug(self.robot.device.state, self.legsError.state)
# self.legsFeatureDes.errorIN.value = self.legsFollower.ldof.value
plug(self.legsFollower.ldof,self.legsFeatureDes.errorIN)
self.legsFeature.jacobianIN.value = self.legsJacobian()
self.legsFeature.setReference(legsFeatureDesName)
plug(self.legsError.error, self.legsFeature.errorIN)
self.legsTask.add(self.legsFeature.name)
self.legsTask.controlGain.value = 5.
#CoM task
print("Com Task")
print (0., 0., self.robot.dynamic.com.value[2])
self.robot.comTask.controlGain.value = 50.
self.robot.featureComDes.errorIN.value = (0., 0., self.robot.dynamic.com.value[2])
self.robot.featureCom.selec.value = '111'
plug(self.legsFollower.com, self.robot.featureComDes.errorIN)
# Plug the legs follower zmp output signals.
plug(self.legsFollower.zmp, self.robot.device.zmp)
solver.sot.remove(self.robot.comTask.name)
print("Push in solver.")
solver.sot.push(self.legsTask.name)
solver.sot.push(self.postureTask.name)
solver.sot.push(self.robot.tasks['waist'].name)
solver.sot.push(self.robot.comTask.name)
solver.sot.remove(self.robot.tasks['left-ankle'].name)
solver.sot.remove(self.robot.tasks['right-ankle'].name)
print solver.sot.display()
print("Tasks added in solver.\n")
print("Command are : \n - f.plug()\n - f.plugViewer()\n - f.plugPlanner()\n"
" - f.plugPlannerWithoutMocap()\n - f.start()\n - f.stop()\n - f.readMocap()\n")
class LegsFollowerGraph(object):
legsFollower = None
postureTask = None
postureFeature = None
postureFeatureDes = None
postureError = None
legsTask = None
legsFeature = None
legsFeatureDes = None
legsError = None
waistTask = None
waistFeature = None
waistFeatureDes = None
waistError = None
trace = None
def __init__(self, robot, solver, trace = None, postureTaskDofs = None):
print("Constructor of LegsFollower Graph")
self.robot = robot
self.solver = solver
self.legsFollower = LegsFollower('legs-follower')
self.statelength = len(robot.device.state.value)
# Initialize the posture task.
print("Posture Task")
self.postureTaskDofs = postureTaskDofs
if not self.postureTaskDofs:
self.postureTaskDofs = []
for i in xrange(len(robot.halfSitting) - 6):
# Disable legs dofs.
if i < 12: #FIXME: not generic enough
self.postureTaskDofs.append((i + 6, False))
else:
self.postureTaskDofs.append((i + 6, True))
# This part is taken from feet_follower_graph
self.postureTask = Task(self.robot.name + '_posture')
self.postureFeature = FeaturePosture(self.robot.name + '_postureFeature')
plug(self.robot.device.state, self.postureFeature.state)
posture = list(self.robot.halfSitting)
self.postureFeature.setPosture(tuple(posture))
for (dof, isEnabled) in self.postureTaskDofs:
self.postureFeature.selectDof(dof, isEnabled)
self.postureTask.add(self.postureFeature.name)
self.postureTask.controlGain.value = 2.
# Initialize the waist follower task.
print("Waist Task")
self.robot.features['waist'].selec.value = '111111'
plug(self.legsFollower.waist, self.robot.features['waist'].reference)
self.robot.tasks['waist'].controlGain.value = 1.
# Initialize the legs follower task.
print("Legs Task")
self.legsTask = Task(self.robot.name + '_legs')
self.legsFeature = FeatureGeneric(self.robot.name + '_legsFeature')
legsFeatureDesName = self.robot.name + '_legsFeatureDes'
self.legsFeatureDes = FeatureGeneric(legsFeatureDesName)
self.legsError = LegsError('LegsError')
plug(self.robot.device.state, self.legsError.state)
# self.legsFeatureDes.errorIN.value = self.legsFollower.ldof.value
plug(self.legsFollower.ldof,self.legsFeatureDes.errorIN)
self.legsFeature.jacobianIN.value = self.legsJacobian()
self.legsFeature.setReference(legsFeatureDesName)
plug(self.legsError.error, self.legsFeature.errorIN)
self.legsTask.add(self.legsFeature.name)
self.legsTask.controlGain.value = 5.
#CoM task
print("Com Task")
print (0., 0., self.robot.dynamic.com.value[2])
self.robot.comTask.controlGain.value = 50.
self.robot.featureComDes.errorIN.value = (0., 0., self.robot.dynamic.com.value[2])
self.robot.featureCom.selec.value = '111'
plug(self.legsFollower.com, self.robot.featureComDes.errorIN)
# Plug the legs follower zmp output signals.
plug(self.legsFollower.zmp, self.robot.device.zmp)
solver.sot.remove(self.robot.comTask.name)
print("Push in solver.")
solver.sot.push(self.legsTask.name)
solver.sot.push(self.postureTask.name)
solver.sot.push(self.robot.tasks['waist'].name)
solver.sot.push(self.robot.comTask.name)
solver.sot.remove(self.robot.tasks['left-ankle'].name)
solver.sot.remove(self.robot.tasks['right-ankle'].name)
print solver.sot.display()
print("Tasks added in solver.\n")
print("Command are : \n - f.plug()\n - f.plugViewer()\n - f.plugPlanner()\n"
" - f.plugPlannerWithoutMocap()\n - f.start()\n - f.stop()\n - f.readMocap()\n")
def legsJacobian(self):
j = []
for i in xrange(12):
j.append(oneVector(6+i,self.statelength))
return tuple(j)
def waistJacobian(self):
j = []
for i in xrange(6):
j.append(oneVector(i,self.statelength))
return tuple(j)
def postureJacobian(self):
j = []
for i in xrange(self.statelength):
if i >= 6 + 2 * 6:
j.append(oneVector(i))
if i == 3 or i == 4:
j.append(zeroVector())
return tuple(j)
def computeDesiredValue(self):
e = self.robot.halfSitting
#e = halfSitting
e_ = [e[3], e[4]]
offset = 6 + 2 * 6
for i in xrange(len(e) - offset):
e_.append(e[offset + i])
return tuple(e_)
def canStart(self):
securityThreshold = 1e-3
return (self.postureTask.error.value <=
(securityThreshold,) * len(self.postureTask.error.value))
def setupTrace(self):
self.trace = TracerRealTime('trace')
self.trace.setBufferSize(2**20)
self.trace.open('/tmp/','legs_follower_','.dat')
self.trace.add('legs-follower.com', 'com')
self.trace.add('legs-follower.zmp', 'zmp')
self.trace.add('legs-follower.ldof', 'ldof')
self.trace.add('legs-follower.waist', 'waist')
self.trace.add(self.robot.device.name + '.state', 'state')
self.trace.add(self.legsTask.name + '.error', 'errorLegs')
self.trace.add(self.robot.comTask.name + '.error', 'errorCom')
#self.trace.add('legs-follower.outputStart','start')
#self.trace.add('legs-follower.outputYaw','yaw')
self.trace.add('corba.planner_steps','steps')
self.trace.add('corba.planner_outputGoal','goal')
self.trace.add('corba.waist','waistMocap')
self.trace.add('corba.left-foot','footMocap')
self.trace.add('corba.table','tableMocap')
self.trace.add('corba.bar','barMocap')
self.trace.add('corba.chair','chairMocap')
self.trace.add('corba.helmet','helmetMocap')
self.trace.add('corba.planner_outputObs','obstacles')
self.trace.add(self.robot.dynamic.name + '.left-ankle',
self.robot.dynamic.name + '-left-ankle')
self.trace.add(self.robot.dynamic.name + '.right-ankle',
self.robot.dynamic.name + '-right-ankle')
# Recompute trace.triger at each iteration to enable tracing.
self.robot.device.after.addSignal('legs-follower.zmp')
self.robot.device.after.addSignal('legs-follower.outputStart')
self.robot.device.after.addSignal('legs-follower.outputYaw')
self.robot.device.after.addSignal('corba.planner_steps')
self.robot.device.after.addSignal('corba.planner_outputGoal')
self.robot.device.after.addSignal('corba.waist')
self.robot.device.after.addSignal('corba.left-foot')
self.robot.device.after.addSignal('corba.table')
self.robot.device.after.addSignal('corba.bar')
self.robot.device.after.addSignal('corba.chair')
self.robot.device.after.addSignal('corba.helmet')
self.robot.device.after.addSignal('corba.planner_outputObs')
self.robot.device.after.addSignal(self.robot.dynamic.name + '.left-ankle')
self.robot.device.after.addSignal(self.robot.dynamic.name + '.right-ankle')
self.robot.device.after.addSignal('trace.triger')
return
def plugPlanner(self):
print("Plug planner.")
plug(corba.planner_radQ, self.legsFollower.inputRef)
plug(self.legsFollower.outputStart, corba.planner_inputStart)
plug(corba.waistTimestamp, corba.planner_timestamp)
plug(corba.table, corba.planner_table)
plug(corba.chair, corba.planner_chair)
plug(corba.bar, corba.planner_bar)
plug(corba.waist, corba.planner_waist)
plug(corba.helmet, corba.planner_inputGoal)
plug(corba.torus1, corba.planner_torus1)
plug(corba.torus2, corba.planner_torus2)
plug(corba.signal('left-foot'), corba.planner_foot)
plug(corba.signal('left-footTimestamp'), corba.planner_footTimestamp)
return
def plugPlannerWithoutMocap(self):
print("Plug planner without mocap.")
plug(corba.planner_radQ, self.legsFollower.inputRef)
plug(self.legsFollower.outputStart, corba.planner_inputStart)
return
def plugViewer(self):
print("Plug viewer.")
plug(self.legsFollower.ldof, corba.viewer_Ldof)
plug(self.legsFollower.outputStart, corba.viewer_Start)
plug(self.legsFollower.com, corba.viewer_Com)
plug(self.legsFollower.outputYaw, corba.viewer_Yaw)
plug(corba.planner_steps, corba.viewer_Steps)
plug(corba.planner_outputGoal, corba.viewer_Goal)
plug(corba.table, corba.viewer_Table)
plug(corba.chair, corba.viewer_Chair)
plug(corba.bar, corba.viewer_Bar)
plug(corba.torus1, corba.viewer_Torus1)
plug(corba.torus2, corba.viewer_Torus2)
plug(corba.waist, corba.viewer_Waist)
plug(corba.planner_outputLabyrinth, corba.viewer_Labyrinth)
plug(corba.planner_outputObs, corba.viewer_Obs)
return
def plug(self):
self.plugPlanner()
self.plugViewer()
return
def readMocap(self):
print "Table : "
print corba.table.value
print "Waist : "
if len(corba.waist.value)<3:
corba.waist.value = (0,0,0)
print corba.waist.value
print "Helmet : "
print corba.helmet.value
return;
def start(self):
if not self.canStart():
print("Robot has not yet converged to the initial position,"
" please wait and try again.")
return
print("Start.")
self.postureTask.controlGain.value = 180.
#self.waistTask.controlGain.value = 90.
self.legsTask.controlGain.value = 180.
self.robot.comTask.controlGain.value = 180.
self.robot.tasks['waist'].controlGain.value = 45.
self.setupTrace()
self.trace.start()
self.legsFollower.start()
return
def stop(self):
self.legsFollower.stop()
self.trace.dump()
return
