def setupJointLimitTask():
featureJl = FeatureJointLimits('featureJl')
featureJl.actuate()
dyn = robot.dynamic
plug(dyn.signal('position'), featureJl.signal('joint'))
plug(dyn.signal('upperJl'), featureJl.signal('upperJl'))
plug(dyn.signal('lowerJl'), featureJl.signal('lowerJl'))
taskJl = Task('taskJl')
taskJl.add('featureJl')
taskJl.signal('controlGain').value = .1
class MetaTaskPose(object):
def opPointExist(self, opPoint):
sigsP = filter(lambda x: x.getName().split(':')[-1] == opPoint,
self.dyn.signals())
sigsJ = filter(lambda x: x.getName().split(':')[-1] == 'J' + opPoint,
self.dyn.signals())
return len(sigsP) == 1 & len(sigsJ) == 1
def defineDynEntities(self, dyn):
self.dyn = dyn
def createOpPoint(self, opPoint, opPointRef='right-wrist'):
self.opPoint = opPoint
if self.opPointExist(opPoint):
return
self.dyn.createOpPoint(opPoint, opPointRef)
def createFeature(self):
self.feature = FeaturePose('feature' + self.name)
self.feature.selec.value = '111111'
def createTask(self):
self.task = Task('task' + self.name)
def createGain(self):
self.gain = GainAdaptive('gain' + self.name)
self.gain.set(0.1, 0.1, 125e3)
def plugEverything(self):
plug(self.dyn.signal(self.opPoint), self.feature.oMjb)
plug(self.dyn.signal('J' + self.opPoint), self.feature.jbJjb)
self.task.add(self.feature.name)
plug(self.task.error, self.gain.error)
plug(self.gain.gain, self.task.controlGain)
def keep(self):
self.feature.faMfb.recompute(self.dyn.position.time)
self.feature.faMfbDes.value = self.feature.faMfb.value
def __init__(self, name, dyn, opPoint, opPointRef='right-wrist'):
self.name = name
self.defineDynEntities(dyn)
self.createOpPoint(opPoint, opPointRef)
self.createFeature()
self.createTask()
self.createGain()
self.plugEverything()
@property
def ref(self):
return self.feature.faMfbDes.value
@ref.setter
def ref(self, m):
self.feature.faMfbDes.value = m
class MotionJoint(Motion):
yaml_tag = u'joint'
gain = None
name = None
reference = None
# FIXME: not generic enough and joints are missing.
nameToId = {
'left-claw': 35,
'right-claw': 28
}
def __init__(self, motion, yamlData, defaultDirectories):
checkDict('interval', yamlData)
Motion.__init__(self, motion, yamlData)
self.gain = yamlData.get('gain', 1.)
self.name = yamlData['name']
self.reference = yamlData['reference']
self.task = Task('joint'+str(id(yamlData)))
self.feature = FeaturePosture('jointFeaturePosture'+str(id(yamlData)))
plug(motion.robot.device.state, self.feature.state)
jointId = self.nameToId[self.name]
posture = np.array(motion.robot.halfSitting)
posture[jointId] = self.reference
self.feature.setPosture(tuple(posture.tolist()))
for i in xrange(len(posture) - 6):
if i + 6 == jointId:
self.feature.selectDof(i + 6, True)
else:
self.feature.selectDof(i + 6, False)
self.task.add(self.feature.name)
self.task.controlGain.value = self.gain
# Push the task into supervisor.
motion.supervisor.addTask(self.task.name,
self.interval[0], self.interval[1],
self.priority,
(jointId,))
def __str__(self):
return "joint motion ({0})".format(self.name)
def setupTrace(self, trace):
pass
def createBalanceTask (self, taskName, gain = 1.):
task = Task (taskName)
task.add (self.featureCom.name)
task.add (self.leftAnkle.name)
task.add (self.rightAnkle.name)
task.controlGain.value = gain
return task
class Posture(Manifold):
def __init__(self, name, sotrobot):
super(Posture, self).__init__()
from dynamic_graph.sot.core import Task, FeatureGeneric, GainAdaptive
from dynamic_graph.sot.core.matrix_util import matrixToTuple
from numpy import identity
n = Posture.sep + name
self.tp = Task('task' + n)
self.tp.dyn = sotrobot.dynamic
self.tp.feature = FeaturePosture('feature_' + n)
q = list(sotrobot.dynamic.position.value)
self.tp.feature.state.value = q
self.tp.feature.posture.value = q
robotDim = sotrobot.dynamic.getDimension()
for i in range(6, robotDim):
self.tp.feature.selectDof(i, True)
self.tp.gain = GainAdaptive("gain_" + n)
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.state)
self.tp.setWithDerivative(True)
# Set the gain of the posture task
setGain(self.tp.gain, (4.9, 0.9, 0.01, 0.9))
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 _signalPositionRef(self):
return self.tp.feature.posture
def _signalVelocityRef(self):
return self.tp.feature.postureDot
def __init__(self, name, filename):
AbstractHumanoidRobot.__init__(self, name)
self.OperationalPoints.append('waist')
p = Parser(self.name + '_dynamic', filename)
self.dynamic = p.parse()
self.dimension = self.dynamic.getDimension()
if self.dimension != len(self.halfSitting):
raise RuntimeError("invalid half-sitting pose")
self.initializeRobot()
self.compoundDrive = CompoundDrive(self.name + '_compoundDrive',
self.device)
self.compoundDriveTask = Task(self.name + '_compoundDriveTask')
self.compoundDriveTask.add(self.name + '_compoundDrive')
self.compoundDriveTask.signal('controlGain').value = 1.
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 __init__(self, motion, yamlData, defaultDirectories):
checkDict('interval', yamlData)
Motion.__init__(self, motion, yamlData)
self.gain = yamlData.get('gain', 1.)
self.name = yamlData['name']
self.reference = yamlData['reference']
self.task = Task('joint'+str(id(yamlData)))
self.feature = FeaturePosture('jointFeaturePosture'+str(id(yamlData)))
plug(motion.robot.device.state, self.feature.state)
jointId = self.nameToId[self.name]
posture = np.array(motion.robot.halfSitting)
posture[jointId] = self.reference
self.feature.setPosture(tuple(posture.tolist()))
for i in xrange(len(posture) - 6):
if i + 6 == jointId:
self.feature.selectDof(i + 6, True)
else:
self.feature.selectDof(i + 6, False)
self.task.add(self.feature.name)
self.task.controlGain.value = self.gain
# Push the task into supervisor.
motion.supervisor.addTask(self.task.name,
self.interval[0], self.interval[1],
self.priority,
(jointId,))
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
def createOperationalPointFeatureAndTask(self,
operationalPointName,
featureName,
taskName,
gain = .2):
jacobianName = 'J{0}'.format(operationalPointName)
self.dynamic.signal(operationalPointName).recompute(0)
self.dynamic.signal(jacobianName).recompute(0)
feature = \
FeaturePosition(featureName,
self.dynamic.signal(operationalPointName),
self.dynamic.signal(jacobianName),
self.dynamic.signal(operationalPointName).value)
task = Task(taskName)
task.add(featureName)
task.controlGain.value = gain
return (feature, task)
def createOperationalPointFeatureAndTask(robot, operationalPointName, featureName, taskName, ingain=.2):
operationalPointMapped = operationalPointName
jacobianName = 'J{0}'.format(operationalPointMapped)
robot.dynamic.signal(operationalPointMapped).recompute(0)
robot.dynamic.signal(jacobianName).recompute(0)
feature = \
FeaturePosition(featureName,
robot.dynamic.signal(operationalPointMapped),
robot.dynamic.signal(jacobianName),
robot.dynamic.signal(operationalPointMapped).value)
task = Task(taskName)
task.add(featureName)
gain = GainAdaptive('gain' + taskName)
gain.setConstant(ingain)
plug(gain.gain, task.controlGain)
plug(task.error, gain.error)
return (feature, task, gain)
def createBalanceTask(robot, application, taskName, ingain=1.):
task = Task(taskName)
task.add(application.featureCom.name)
task.add(application.leftAnkle.name)
task.add(application.rightAnkle.name)
gain = GainAdaptive('gain' + taskName)
gain.setConstant(ingain)
plug(gain.gain, task.controlGain)
plug(task.error, gain.error)
return (task, gain)
class EEPosture (Manifold):
def __init__ (self, sotrobot, gripper, position):
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
super(EEPosture, self).__init__()
self.gripper = gripper
# Get joint position in posture
pinmodel = sotrobot.dynamic.model
idJ = pinmodel.getJointId(gripper.sotjoint)
assert idJ < pinmodel.njoints
joint = sotrobot.dynamic.model.joints[idJ]
assert joint.nq == len(position)
idx_q = joint.idx_q + 1
idx_v = joint.idx_v + 1
n = "eeposture" + Posture.sep + gripper.name + Posture.sep + str(position)
self.tp = Task ('task' + n)
self.tp.dyn = sotrobot.dynamic
self.tp.feature = FeaturePosture ('feature_' + n)
plug(sotrobot.dynamic.position, self.tp.feature.state)
q = list(sotrobot.dynamic.position.value)
q[idx_v:idx_v + joint.nv] = position
self.tp.feature.posture.value = q
self.tp.gain = GainAdaptive("gain_"+n)
robotDim = sotrobot.dynamic.getDimension()
# for i in range (6, robotDim):
# self.tp.feature.selectDof (i, False)
# print idx_q, idx_v
for i in range(joint.nv):
self.tp.feature.selectDof (idx_v + i, True)
self.tp.add(self.tp.feature.name)
# Set the gain of the posture task
setGain(self.tp.gain,(4.9,0.9,0.01,0.9))
plug(self.tp.gain.gain, self.tp.controlGain)
plug(self.tp.error, self.tp.gain.error)
self.tasks = [ self.tp ]
class EEPosture(Manifold):
def __init__(self, sotrobot, gripper, position):
from dynamic_graph.sot.core import Task, GainAdaptive
super(EEPosture, self).__init__()
self.gripper = gripper
self.jointNames = gripper.joints
pinmodel = sotrobot.dynamic.model
n = "eeposture" + Posture.sep + gripper.name + Posture.sep + str(
list(position))
self.tp = Task('task' + n)
self.tp.dyn = sotrobot.dynamic
self.tp.feature = FeaturePosture('feature_' + n)
plug(sotrobot.dynamic.position, self.tp.feature.state)
q = list(sotrobot.dynamic.position.value)
# Define the reference and the selected DoF
rank = 0
ranks = []
for name in self.jointNames:
idJ = pinmodel.getJointId(name)
assert idJ < pinmodel.njoints
joint = pinmodel.joints[idJ]
idx_v = joint.idx_v
nv = joint.nv
ranks += range(idx_v, idx_v + nv)
q[idx_v:idx_v + nv] = position[rank:rank + nv]
rank += nv
assert rank == len(position)
self.tp.feature.posture.value = q
for i in ranks:
self.tp.feature.selectDof(i, True)
self.tp.gain = GainAdaptive("gain_" + n)
self.tp.add(self.tp.feature.name)
# Set the gain of the posture task
setGain(self.tp.gain, (4.9, 0.9, 0.01, 0.9))
plug(self.tp.gain.gain, self.tp.controlGain)
plug(self.tp.error, self.tp.gain.error)
if len(self.jointNames) > 0:
self.tasks = [self.tp]
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)
class MetaTaskKine6d(MetaTask6d):
def createTask(self):
self.task = Task('task' + self.name)
def createGain(self):
self.gain = GainAdaptive('gain' + self.name)
self.gain.set(0.1, 0.1, 125e3)
def plugEverything(self):
self.feature.setReference(self.featureDes.name)
plug(self.dyn.signal(self.opPoint), self.feature.signal('position'))
plug(self.dyn.signal('J' + self.opPoint), self.feature.signal('Jq'))
self.task.add(self.feature.name)
plug(self.task.error, self.gain.error)
plug(self.gain.gain, self.task.controlGain)
def keep(self):
self.feature.position.recompute(self.dyn.position.time)
self.feature.keep()
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)
class Posture(Manifold):
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 _signalPositionRef (self): return self.tp.featureDes.errorIN
def _signalVelocityRef (self): return self.tp.featureDes.errordotIN
def __init__(self, sotrobot, gripper, name_suffix):
from dynamic_graph.sot.core import Task, GainAdaptive
super(EndEffector, self).__init__()
self.gripper = gripper
self.jointNames = gripper.joints
self.robot = sotrobot
pinmodel = sotrobot.dynamic.model
self.name = Posture.sep.join(["ee", gripper.name, name_suffix])
self.tp = Task('task' + self.name)
self.tp.dyn = sotrobot.dynamic
self.tp.feature = FeaturePosture('feature_' + self.name)
plug(sotrobot.dynamic.position, self.tp.feature.state)
# Select the dofs
self.tp.feature.posture.value = sotrobot.dynamic.position.value
# Define the reference and the selected DoF
self.jointRanks = []
for name in self.jointNames:
idJ = pinmodel.getJointId(name)
assert idJ < pinmodel.njoints
joint = pinmodel.joints[idJ]
idx_v = joint.idx_v
nv = joint.nv
self.jointRanks.append((idx_v, nv))
for idx_v, nv in self.jointRanks:
for i in range(idx_v, idx_v + nv):
self.tp.feature.selectDof(i, True)
self.tp.gain = GainAdaptive("gain_" + self.name)
self.tp.add(self.tp.feature.name)
# Set the gain of the posture task
setGain(self.tp.gain, (4.9, 0.9, 0.01, 0.9))
plug(self.tp.gain.gain, self.tp.controlGain)
plug(self.tp.error, self.tp.gain.error)
if len(self.jointNames) > 0:
self.tasks = [self.tp]
def createOperationalPointFeatureAndTask(robot,
operationalPointName,
featureName,
taskName,
ingain=.2):
operationalPointMapped = operationalPointName
jacobianName = 'J{0}'.format(operationalPointMapped)
robot.dynamic.signal(operationalPointMapped).recompute(0)
robot.dynamic.signal(jacobianName).recompute(0)
feature = \
FeaturePosition(featureName,
robot.dynamic.signal(operationalPointMapped),
robot.dynamic.signal(jacobianName),
robot.dynamic.signal(operationalPointMapped).value)
task = Task(taskName)
task.add(featureName)
gain = GainAdaptive('gain' + taskName)
gain.setConstant(ingain)
plug(gain.gain, task.controlGain)
plug(task.error, gain.error)
return (feature, task, gain)
def createBalanceTask (robot, application, taskName, ingain = 1.):
task = Task (taskName)
task.add (application.featureCom.name)
task.add (application.leftAnkle.name)
task.add (application.rightAnkle.name)
gain = GainAdaptive('gain'+taskName)
gain.setConstant(ingain)
plug(gain.gain, task.controlGain)
plug(task.error, gain.error)
return (task, gain)
def __init__(self, motion, yamlData, defaultDirectories):
checkDict('interval', yamlData)
Motion.__init__(self, motion, yamlData)
self.objectName = yamlData['object-name']
self.frameName = yamlData['frame-name']
self.gain = yamlData.get('gain', 1.)
# Cannot change the stack dynamically for now.
if self.interval[0] != 0 and self.interval[1] != motion.duration:
raise NotImplementedError
# Desired feature
self.fvpDes = FeatureVisualPoint('fvpDes'+str(id(yamlData)))
self.fvpDes.xy.value = (0., 0.)
# Feature
self.vispPointProjection = VispPointProjection('vpp'+str(id(yamlData)))
self.vispPointProjection.cMo.value = (
(1., 0., 0., 0.),
(0., 1., 0., 0.),
(0., 0., 1., 1.),
(0., 0., 0., 1.),)
self.vispPointProjection.cMoTimestamp.value = (0., 0.)
self.fvp = FeatureVisualPoint('fvp'+str(id(yamlData)))
plug(self.vispPointProjection.xy, self.fvp.xy)
plug(self.vispPointProjection.Z, self.fvp.Z)
self.fvp.Z.value = 1.
self.fvp.sdes.value = self.fvpDes
self.fvp.selec.value = '11'
plug(motion.robot.frames[self.frameName].jacobian, self.fvp.Jq)
self.fvp.error.recompute(self.fvp.error.time + 1)
self.fvp.jacobian.recompute(self.fvp.jacobian.time + 1)
# Task
self.task = Task('task_fvp_'+str(id(yamlData)))
self.task.add(self.fvp.name)
self.task.controlGain.value = self.gain
self.task.error.recompute(self.task.error.time + 1)
self.task.jacobian.recompute(self.task.jacobian.time + 1)
# Push the task into supervisor.
motion.supervisor.addTask(self.task.name,
self.interval[0], self.interval[1],
self.priority,
#FIXME: HRP-2 specific
(6 + 14, 6 + 15))
class Nao(AbstractHumanoidRobot):
"""
This class instanciates a Nao robot as a humanoid robot
"""
halfSitting = tuple([0.,0.,.31,0.,0.,0.] + halfSitting)
def __init__(self, name, filename):
AbstractHumanoidRobot.__init__(self, name)
self.OperationalPoints.append('waist')
p = Parser(self.name + '_dynamic', filename)
self.dynamic = p.parse()
self.dimension = self.dynamic.getDimension()
if self.dimension != len(self.halfSitting):
raise RuntimeError("invalid half-sitting pose")
self.initializeRobot()
self.compoundDrive = CompoundDrive(self.name + '_compoundDrive',
self.device)
self.compoundDriveTask = Task(self.name + '_compoundDriveTask')
self.compoundDriveTask.add(self.name + '_compoundDrive')
self.compoundDriveTask.signal('controlGain').value = 1.
def setupCustomOpPointTask(op):
robot.dynamic.createCustomOpPoint(op, 27, (0, 0, -0.2))
robot.dynamic.signal(op).recompute(0)
robot.dynamic.signal('J' + op).recompute(0)
robot.features[op] = \
FeaturePosition(robot.name + '_feature_' + op,
robot.dynamic.signal(op),
robot.dynamic.signal('J' + op),
robot.dynamic.signal(op).value)
robot.tasks[op] = Task(robot.name + '_task_' + op)
robot.tasks[op].add(robot.name + '_feature_' + op)
robot.tasks[op].controlGain.value = .2
robot.OperationalPoints.append(op)
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)
# flake8: noqa
from dynamic_graph import plug
from dynamic_graph.sot.core import SOT, FeatureGeneric, FeaturePosture, GainAdaptive, Task
from dynamic_graph.sot.core.matrix_util import matrixToTuple
from dynamic_graph.sot.core.meta_tasks import setGain
from dynamic_graph.sot.core.meta_tasks_kine import MetaTaskKine6d, MetaTaskKineCom, gotoNd
from dynamic_graph.sot.tiago.diff_drive_controller import HolonomicProjection
n = "posture"
tp = Task('task' + n)
tp.dyn = robot.dynamic
tp.feature = FeaturePosture('feature_' + n)
q = list(robot.device.robotState.value)
tp.feature.state.value = q
tp.feature.posture.value = q
robotDim = robot.dynamic.getDimension()
assert robotDim == len(q)
with_wheels = robotDim == 19
for i in range(8 if with_wheels else 6, robotDim):
tp.feature.selectDof(i, True)
tp.gain = GainAdaptive("gain_" + n)
tp.add(tp.feature.name)
# Connects the dynamics to the current feature of the posture task
plug(robot.dynamic.position, tp.feature.state)
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
#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)
from dynamic_graph.sot.core.meta_tasks_kine import MetaTaskKine6d
from dynamic_graph.sot.dyninv import SolverKine
robot = youbot('youbot', device=RobotSimu('youbot'))
#robot = Pr2('youbot', device=RobotSimu('youbot'))
dimension = robot.dynamic.getDimension()
robot.device.resize (dimension)
from dynamic_graph.ros import Ros
ros = Ros(robot)
#waisttask-1
robot_pose = ((1.,0,0,0.4),
(0,1.,0,0),
(0,0,1.,0.0),
(0,0,0,1.),)
feature_waist = FeaturePosition ('position_waist', robot.dynamic.base_joint,robot.dynamic.Jbase_joint, robot_pose)
task_waist = Task ('waist_task')
task_waist.controlGain.value = 0.5
task_waist.add (feature_waist.name)
#waisttask
task_waist_metakine=MetaTaskKine6d('task_waist_metakine',robot.dynamic,'base_joint','base_joint')
goal_waist = ((1.,0,0,0.0),(0,1.,0,-0.0),(0,0,1.,0.0),(0,0,0,1.),)
task_waist_metakine.feature.frame('desired')
#task_waist_metakine.feature.selec.value = '011101'#RzRyRxTzTyTx
task_waist_metakine.gain.setConstant(0.5)
task_waist_metakine.featureDes.position.value = goal_waist
solver = SolverKine('sot_solver')
solver.setSize (robot.dynamic.getDimension())
robot.device.resize (robot.dynamic.getDimension())
'''
#taskinequality
def __init__(self, dyn, config=None, name="config"):
MetaTaskConfig.__init__(self, dyn, config, name)
self.task = Task('task' + name)
self.plugTask()
class MotionVisualPoint(Motion):
yaml_tag = u'visual-point'
type = None
gain = None
objectName = None
def __init__(self, motion, yamlData, defaultDirectories):
checkDict('interval', yamlData)
Motion.__init__(self, motion, yamlData)
self.objectName = yamlData['object-name']
self.frameName = yamlData['frame-name']
self.gain = yamlData.get('gain', 1.)
# Cannot change the stack dynamically for now.
if self.interval[0] != 0 and self.interval[1] != motion.duration:
raise NotImplementedError
# Desired feature
self.fvpDes = FeatureVisualPoint('fvpDes'+str(id(yamlData)))
self.fvpDes.xy.value = (0., 0.)
# Feature
self.vispPointProjection = VispPointProjection('vpp'+str(id(yamlData)))
self.vispPointProjection.cMo.value = (
(1., 0., 0., 0.),
(0., 1., 0., 0.),
(0., 0., 1., 1.),
(0., 0., 0., 1.),)
self.vispPointProjection.cMoTimestamp.value = (0., 0.)
self.fvp = FeatureVisualPoint('fvp'+str(id(yamlData)))
plug(self.vispPointProjection.xy, self.fvp.xy)
plug(self.vispPointProjection.Z, self.fvp.Z)
self.fvp.Z.value = 1.
self.fvp.sdes.value = self.fvpDes
self.fvp.selec.value = '11'
plug(motion.robot.frames[self.frameName].jacobian, self.fvp.Jq)
self.fvp.error.recompute(self.fvp.error.time + 1)
self.fvp.jacobian.recompute(self.fvp.jacobian.time + 1)
# Task
self.task = Task('task_fvp_'+str(id(yamlData)))
self.task.add(self.fvp.name)
self.task.controlGain.value = self.gain
self.task.error.recompute(self.task.error.time + 1)
self.task.jacobian.recompute(self.task.jacobian.time + 1)
# Push the task into supervisor.
motion.supervisor.addTask(self.task.name,
self.interval[0], self.interval[1],
self.priority,
#FIXME: HRP-2 specific
(6 + 14, 6 + 15))
def __str__(self):
msg = "visual point motion (frame: {0}, object: {1})"
return msg.format(self.frameName, self.objectName)
def setupTrace(self, trace):
for s in ['xy', 'Z']:
addTrace(self.robot, trace, self.vispPointProjection.name, s)
for s in ['xy']:
addTrace(self.robot, trace, self.fvpDes.name, s)
for s in ['xy', 'Z', 'error']:
addTrace(self.robot, trace, self.fvp.name, s)
for s in ['error']:
addTrace(self.robot, trace, self.task.name, s)
class EndEffector(Manifold):
def __init__(self, sotrobot, gripper, name_suffix):
from dynamic_graph.sot.core import Task, GainAdaptive
super(EndEffector, self).__init__()
self.gripper = gripper
self.jointNames = gripper.joints
self.robot = sotrobot
pinmodel = sotrobot.dynamic.model
self.name = Posture.sep.join(["ee", gripper.name, name_suffix])
self.tp = Task('task' + self.name)
self.tp.dyn = sotrobot.dynamic
self.tp.feature = FeaturePosture('feature_' + self.name)
plug(sotrobot.dynamic.position, self.tp.feature.state)
# Select the dofs
self.tp.feature.posture.value = sotrobot.dynamic.position.value
# Define the reference and the selected DoF
self.jointRanks = []
for name in self.jointNames:
idJ = pinmodel.getJointId(name)
assert idJ < pinmodel.njoints
joint = pinmodel.joints[idJ]
idx_v = joint.idx_v
nv = joint.nv
self.jointRanks.append((idx_v, nv))
for idx_v, nv in self.jointRanks:
for i in range(idx_v, idx_v + nv):
self.tp.feature.selectDof(i, True)
self.tp.gain = GainAdaptive("gain_" + self.name)
self.tp.add(self.tp.feature.name)
# Set the gain of the posture task
setGain(self.tp.gain, (4.9, 0.9, 0.01, 0.9))
plug(self.tp.gain.gain, self.tp.controlGain)
plug(self.tp.error, self.tp.gain.error)
if len(self.jointNames) > 0:
self.tasks = [self.tp]
### \param type equals "open" or "close"
### \param period interval between two integration of SoT
def makeAdmittanceControl(self,
affordance,
type,
period,
simulateTorqueFeedback=False,
filterCurrents=True):
# Make the admittance controller
from agimus_sot.control.gripper import AdmittanceControl, PositionAndAdmittanceControl
from .events import norm_superior_to
# type = "open" or "close"
desired_torque = affordance.ref["torque"]
estimated_theta_close = affordance.ref["angle_close"]
wn, z, nums, denoms = affordance.getControlParameter()
if affordance.controlType[type] == "torque":
self.ac = AdmittanceControl("AC_" + self.name + "_" + type,
estimated_theta_close, desired_torque,
period, nums, denoms)
elif affordance.controlType[type] == "position_torque":
theta_open = affordance.ref["angle_open"]
threshold_up = tuple([x / 10. for x in desired_torque])
threshold_down = tuple([x / 100. for x in desired_torque])
self.ac = PositionAndAdmittanceControl(
"AC_" + self.name + "_" + type, theta_open,
estimated_theta_close, desired_torque, period, threshold_up,
threshold_down, wn, z, nums, denoms)
else:
raise NotImplementedError("Control type " + type +
" is not implemented for gripper.")
if simulateTorqueFeedback:
# Get torque from an internal simulation
M, d, k, x0 = affordance.getSimulationParameters()
self.ac.setupFeedbackSimulation(M, d, k, x0)
# Must be done after setupFeedbackSimulation
self.ac.readPositionsFromRobot(self.robot, self.jointNames)
else:
self.ac.readPositionsFromRobot(self.robot, self.jointNames)
# Get torque from robot sensors (or external simulation)
# TODO allows to switch between current and torque sensors
self.ac.readCurrentsFromRobot(self.robot, self.jointNames,
(self.gripper.torque_constant, ),
filterCurrents)
# self.ac.readTorquesFromRobot(self.robot, self.jointNames)
from dynamic_graph.sot.core.operator import Mix_of_vector
mix_of_vector = Mix_of_vector(self.name + "_control_to_robot_control")
mix_of_vector.default.value = tuple([
0,
] * len(self.tp.feature.posture.value))
mix_of_vector.setSignalNumber(2)
for idx_v, nv in self.jointRanks:
mix_of_vector.addSelec(1, idx_v, nv)
# Plug the admittance controller to the posture task
setGain(self.tp.gain, 1.)
plug(self.ac.outputPosition, mix_of_vector.signal("sin1"))
plug(mix_of_vector.sout, self.tp.feature.posture)
# TODO plug posture dot ?
# I do not think it is necessary.
# TODO should we send to the posture task
# - the current robot posture
# - the postureDot from self.ac.outputDerivative
# This would avoid the last integration in self.ac.
# This integration does not know the initial point so
# there might be some drift (removed the position controller at the beginning)
n, c = norm_superior_to(self.name + "_torquecmp",
self.ac.currentTorqueIn,
0.9 * np.linalg.norm(desired_torque))
self.events = {
"done_close": c.sout,
}
def makePositionControl(self, position):
q = list(self.tp.feature.state.value)
# Define the reference
ip = 0
for iq, nv in self.jointRanks:
q[iq:iq + nv] = position[ip:ip + nv]
ip += nv
assert ip == len(position)
self.tp.feature.posture.value = q
setGain(self.tp.gain, (4.9, 0.9, 0.01, 0.9))
from .events import norm_inferior_to
n, c = norm_inferior_to(self.name + "_positioncmp", self.tp.error,
0.001)
self.events = {
"done_close": c.sout,
"done_open": c.sout,
}
def __init__(self, robot, solver, trace = None):
self.legsFollower = LegsFollower('legs-follower')
# Initialize the posture task.
print("Posture Task")
self.postureTask = Task(robot.name + '_posture')
self.postureFeature = FeatureGeneric(robot.name + '_postureFeature')
self.postureFeatureDes = FeatureGeneric(robot.name + '_postureFeatureDes')
self.postureError = PostureError('PostureError')
plug(robot.device.state, self.postureError.state)
plug(self.postureError.error, self.postureFeature.errorIN)
self.postureFeature.jacobianIN.value = self.postureJacobian()
self.postureFeatureDes.errorIN.value = self.computeDesiredValue()
self.postureFeature.sdes.value = self.postureFeatureDes
self.postureTask.add(self.postureFeature.name)
self.postureTask.controlGain.value = 2.
# Initialize the waist follower task.
print("Waist Task")
robot.features['waist'].selec.value = '111111'
plug(self.legsFollower.waist, robot.features['waist'].reference)
robot.tasks['waist'].controlGain.value = 1.
#self.waistTask = Task(robot.name + '_waist')
#self.waistFeature = FeatureGeneric(robot.name + '_waistFeature')
#self.waistFeatureDes = FeatureGeneric(robot.name + '_waistsFeatureDes')
#self.waistError = WaistError('WaistError')
#plug(robot.device.state, self.waistError.state)
#plug(self.waistError.error, self.waistFeature.errorIN)
#self.waistFeature.jacobianIN.value = self.waistJacobian()
#plug(self.legsFollower.waist, self.waistFeatureDes.errorIN)
#self.waistFeature.sdes.value = self.waistFeatureDes
#self.waistTask.add(self.waistFeature.name)
#self.waistTask.controlGain.value = 1.
# Initialize the legs follower task.
print("Legs Task")
self.legsTask = Task(robot.name + '_legs')
self.legsFeature = FeatureGeneric(robot.name + '_legsFeature')
self.legsFeatureDes = FeatureGeneric(robot.name + '_legsFeatureDes')
self.legsError = LegsError('LegsError')
plug(robot.device.state, self.legsError.state)
plug(self.legsError.error, self.legsFeature.errorIN)
self.legsFeature.jacobianIN.value = self.legsJacobian()
plug(self.legsFollower.ldof, self.legsFeatureDes.errorIN)
self.legsFeature.sdes.value = self.legsFeatureDes
self.legsTask.add(self.legsFeature.name)
self.legsTask.controlGain.value = 5.
#CoM task
print("Com Task")
print (0., 0., robot.dynamic.com.value[2])
robot.comTask.controlGain.value = 50.
robot.featureComDes.errorIN.value = (0., 0., robot.dynamic.com.value[2])
robot.featureCom.selec.value = '111'
plug(self.legsFollower.com, robot.featureComDes.errorIN)
# Plug the legs follower zmp output signals.
plug(self.legsFollower.zmp, robot.device.zmp)
solver.sot.remove(robot.comTask.name)
print("Push in solver.")
solver.sot.push(self.legsTask.name)
solver.sot.push(self.postureTask.name)
solver.sot.push(robot.tasks['waist'].name)
solver.sot.push(robot.comTask.name)
solver.sot.remove(robot.tasks['left-ankle'].name)
solver.sot.remove(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")
solver.sot.push(robot.name + '_task_right-wrist')
# Feature visual point
fvp = FeatureVisualPoint('fvp')
fvp.xy.value = (P(S(0,0), Xw(0))[0], P(S(0,0), Xw(0))[1])
fvp.Z.value = 1. + np.inner(np.linalg.inv(S(0,0)), Xw(0))[2]
plug(w_M_c.jacobian, fvp.Jq)
fvp_sdes = FeatureVisualPoint('fvp_sdes')
fvp_sdes.xy.value = (0., 0.)
fvp.sdes.value = fvp_sdes
task = Task('fvp_task')
task.add('fvp')
task.controlGain.value = 10.
solver.sot.push('fvp_task')
t = 0
for i in xrange(0,2000):
robot.device.increment(timeStep)
w_M_c.position.recompute(t)
w_M_c.jacobian.recompute(t)
fvp.xy.value = (P(S(0,0), Xw(t))[0], P(S(0,0), Xw(t))[1])
if clt:
def __init__(self, dyn, name="com"):
MetaTaskCom.__init__(self, dyn, name)
self.task = Task('task' + name)
self.plugTask()
#task_waist_metakine.feature.selec.value = '111101'#RzRyRxTzTyTx
task_waist_metakine.gain.setConstant(1)
task_waist_metakine.featureDes.position.value = goal_waist
##POSTURE TASK###
posture_feature = FeaturePosture('featurePosition')
plug(robot.device.state,posture_feature.state)
robotDim = len(robot.dynamic.velocity.value)
posture_feature.posture.value = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
postureTaskDofs = [True]*(36)
for dof,isEnabled in enumerate(postureTaskDofs):
if dof > 6:
posture_feature.selectDof(dof,isEnabled)
#robot.features['featurePosition'].selectDof(dof,isEnabled)
task_posture=Task('robot_task_position')
task_posture.add(posture_feature.name)
# featurePosition.selec.value = toFlags((6,24))
gainPosition = GainAdaptive('gainPosition')
gainPosition.set(0.1,0.1,125e3)
gainPosition.gain.value = 5
plug(task_posture.error,gainPosition.error)
plug(gainPosition.gain,task_posture.controlGain)
solver.damping.value =3e-2
solver.push (joint_limit_task.name)
time.sleep(1)
solver.push (task_waist_metakine.task.name)
time.sleep(1)
solver.push (task_posture.name)
time.sleep(1)
plug (solver.control,robot.device.control)
trace.add(signal, filename)
if autoRecompute:
device.after.addSignal(signal)
# Create task for the waist
robot_pose = (
(1., 0, 0, 0),
(0, 1., 0, 0),
(0, 0, 1., 0.089159),
(0, 0, 0, 1.),
)
feature_waist = FeaturePosition('position_waist',
robot.dynamic.shoulder_pan_joint,
robot.dynamic.Jshoulder_pan_joint, robot_pose)
task_waist = Task('waist_task')
task_waist.controlGain.value = 100.
task_waist.add(feature_waist.name)
# Create task for the lift
I4 = (
(1., 0, 0, 0.0),
(0, 1., 0, 0.136),
(0, 0, 1., 0.089),
(0, 0, 0, 1.),
)
feature_lift = FeaturePosition('position_lift',
robot.dynamic.shoulder_lift_joint,
robot.dynamic.Jshoulder_lift_joint, I4)
#feature_lift.selec.value = '000000'
task_lift = Task('lift_task')
(0, 1, 0, anklePos[1]),
(0, 0, 1, anklePos[2]),
(0, 0, 0, 1))
MR = ((1, 0, 0, anklePos[0] + .5 * soleW),
(0, 1, 0, -anklePos[1]),
(0, 0, 1, anklePos[2]),
(0, 0, 0, 1))
feetFollower.signal('feetToAnkleLeft').value = ML
feetFollower.signal('feetToAnkleRight').value = MR
robot.featureCom.selec.value = '111'
###############################################################
# Half sitting task
taskHalfSit = Task(robot.name + '_halfSit')
featureHS = FeatureGeneric('featureHS')
featureHSdes = FeatureGeneric('featureHSdes')
plug(robot.dynamic.position, featureHS.errorIN)
JHS = MatrixConstant('JHS')
JHS.resize(36, 36)
eye = []
for i in xrange(36):
tmp = []
for j in xrange(36):
if i != j:
tmp.append(0.)
else:
tmp.append(1.)
eye.append(tuple(tmp))
controller.tauDes.value = [0.0]
controller.init(timeStep, 1)
controller.setPosition([robot.device.state.value[LeftRollJoint + 6]])
robot.leftRollAnkleController = controller
robot.leftAnkleRollTask = MetaTaskKineJoint(robot.dynamic, LeftRollJoint)
robot.leftAnkleRollTask.task.controlGain.value = 0
robot.leftAnkleRollTask.task.setWithDerivative(True)
plug(robot.leftRollAnkleController.qRef,
robot.leftAnkleRollTask.featureDes.errorIN)
plug(robot.leftRollAnkleController.dqRef,
robot.leftAnkleRollTask.featureDes.errordotIN)
# -------------------------- SOT CONTROL --------------------------
# --- Posture
robot.taskPosture = Task('taskPosture')
robot.taskPosture.feature = FeaturePosture('featurePosture')
q = list(robot.dynamic.position.value)
robot.taskPosture.feature.state.value = q
robot.taskPosture.feature.posture.value = q
for i in range(18, 38):
robot.taskPosture.feature.selectDof(i, True)
robot.taskPosture.controlGain.value = 100.0
robot.taskPosture.add(robot.taskPosture.feature.name)
plug(robot.dynamic.position, robot.taskPosture.feature.state)
# --- CONTACTS
# define contactLF and contactRF
"""
Add a signal to a tracer and recompute it automatically if necessary.
"""
signal = '{0}.{1}'.format(entityName, signalName)
filename = '{0}-{1}'.format(entityName, signalName)
trace.add(signal, filename)
if autoRecompute:
device.after.addSignal(signal)
# Create task for the waist
robot_pose = ((1.,0,0,0),
(0,1.,0,0),
(0,0,1.,0.089159),
(0,0,0,1.),)
feature_waist = FeaturePosition ('position_waist', robot.dynamic.shoulder_pan_joint,robot.dynamic.Jshoulder_pan_joint, robot_pose)
task_waist = Task ('waist_task')
task_waist.controlGain.value = 100.
task_waist.add (feature_waist.name)
# Create task for the lift
I4 = ((1.,0,0,0.0),
(0,1.,0,0.136),
(0,0,1.,0.089),
(0,0,0,1.),)
feature_lift = FeaturePosition ('position_lift', robot.dynamic.shoulder_lift_joint, robot.dynamic.Jshoulder_lift_joint, I4)
#feature_lift.selec.value = '000000'
task_lift = Task ('lift_task')
task_lift.controlGain.value = 0.
task_lift.add (feature_lift.name)
# Create task for the elbow
def createTask(self):
self.task = Task('task' + self.name)
class MetaTask6d(object):
name = ''
opPoint = ''
dyn = 0
task = 0
feature = 0
featureDes = 0
def opPointExist(self, opPoint):
sigsP = filter(lambda x: x.getName().split(':')[-1] == opPoint, self.dyn.signals())
sigsJ = filter(lambda x: x.getName().split(':')[-1] == 'J' + opPoint, self.dyn.signals())
return len(sigsP) == 1 & len(sigsJ) == 1
def defineDynEntities(self, dyn):
self.dyn = dyn
def createOpPoint(self, opPoint, opPointRef='right-wrist'):
self.opPoint = opPoint
if self.opPointExist(opPoint):
return
self.dyn.createOpPoint(opPoint, opPointRef)
def createOpPointModif(self):
self.opPointModif = OpPointModifier('opmodif' + self.name)
plug(self.dyn.signal(self.opPoint), self.opPointModif.signal('positionIN'))
plug(self.dyn.signal('J' + self.opPoint), self.opPointModif.signal('jacobianIN'))
self.opPointModif.activ = False
def createFeatures(self):
self.feature = FeaturePoint6d('feature' + self.name)
self.featureDes = FeaturePoint6d('feature' + self.name + '_ref')
self.feature.selec.value = '111111'
self.feature.frame('current')
def createTask(self):
self.task = Task('task' + self.name)
def createGain(self):
self.gain = GainAdaptive('gain' + self.name)
self.gain.set(0.1, 0.1, 125e3)
def plugEverything(self):
self.feature.setReference(self.featureDes.name)
plug(self.dyn.signal(self.opPoint), self.feature.signal('position'))
plug(self.dyn.signal('J' + self.opPoint), self.feature.signal('Jq'))
self.task.add(self.feature.name)
plug(self.task.error, self.gain.error)
plug(self.gain.gain, self.task.controlGain)
def keep(self):
self.feature.position.recompute(self.dyn.position.time)
self.feature.keep()
def __init__(self, name, dyn, opPoint, opPointRef='right-wrist'):
self.name = name
self.defineDynEntities(dyn)
self.createOpPoint(opPoint, opPointRef)
self.createOpPointModif()
self.createFeatures()
self.createTask()
self.createGain()
self.plugEverything()
@property
def ref(self):
return self.featureDes.position.value
@ref.setter
def ref(self, m):
self.featureDes.position.value = m
@property
def opmodif(self):
if not self.opPointModif.activ:
return False
else:
return self.opPointModif.getTransformation()
@opmodif.setter
def opmodif(self, m):
if isinstance(m, bool) and not m:
plug(self.dyn.signal(self.opPoint), self.feature.signal('position'))
plug(self.dyn.signal('J' + self.opPoint), self.feature.signal('Jq'))
self.opPointModif.activ = False
else:
if not self.opPointModif.activ:
plug(self.opPointModif.signal('position'), self.feature.position)
plug(self.opPointModif.signal('jacobian'), self.feature.Jq)
self.opPointModif.setTransformation(m)
self.opPointModif.activ = True
class MetaTaskVisualPoint(object):
name = ''
opPoint = ''
dyn = 0
task = 0
feature = 0
featureDes = 0
proj = 0
def opPointExist(self, opPoint):
sigsP = filter(lambda x: x.getName().split(':')[-1] == opPoint,
self.dyn.signals())
sigsJ = filter(lambda x: x.getName().split(':')[-1] == 'J' + opPoint,
self.dyn.signals())
return len(sigsP) == 1 & len(sigsJ) == 1
def defineDynEntities(self, dyn):
self.dyn = dyn
def createOpPoint(self, opPoint, opPointRef='right-wrist'):
self.opPoint = opPoint
if self.opPointExist(opPoint):
return
self.dyn.createOpPoint(opPoint, opPointRef)
def createOpPointModif(self):
self.opPointModif = OpPointModifier('opmodif' + self.name)
plug(self.dyn.signal(self.opPoint),
self.opPointModif.signal('positionIN'))
plug(self.dyn.signal('J' + self.opPoint),
self.opPointModif.signal('jacobianIN'))
self.opPointModif.activ = False
def createFeatures(self):
self.feature = FeatureVisualPoint('feature' + self.name)
self.featureDes = FeatureVisualPoint('feature' + self.name + '_ref')
self.feature.selec.value = '11'
def createTask(self):
self.task = Task('task' + self.name)
def createGain(self):
self.gain = GainAdaptive('gain' + self.name)
self.gain.set(0.1, 0.1, 125e3)
def createProj(self):
self.proj = VisualPointProjecter('proj' + self.name)
def plugEverything(self):
self.feature.setReference(self.featureDes.name)
plug(self.dyn.signal(self.opPoint), self.proj.signal('transfo'))
plug(self.proj.signal('point2D'), self.feature.signal('xy'))
plug(self.proj.signal('depth'), self.feature.signal('Z'))
plug(self.dyn.signal('J' + self.opPoint), self.feature.signal('Jq'))
self.task.add(self.feature.name)
plug(self.task.error, self.gain.error)
plug(self.gain.gain, self.task.controlGain)
def __init__(self, name, dyn, opPoint, opPointRef='right-wrist'):
self.name = name
self.defineDynEntities(dyn)
self.createOpPoint(opPoint, opPointRef)
self.createOpPointModif()
self.createFeatures()
self.createTask()
self.createGain()
self.createProj()
self.plugEverything()
@property
def ref(self):
return self.featureDes.xy.value
@ref.setter
def ref(self, m):
self.featureDes.xy.value = m
@property
def target(self):
return self.proj.point3D
@target.setter
def target(self, m):
self.proj.point3D.value = m
@property
def opmodif(self):
if not self.opPointModif.activ:
return False
else:
return self.opPointModif.getTransformation()
@opmodif.setter
def opmodif(self, m):
if isinstance(m, bool) and not m:
plug(self.dyn.signal(self.opPoint), self.proj.signal('transfo'))
plug(self.dyn.signal('J' + self.opPoint),
self.feature.signal('Jq'))
self.opPointModif.activ = False
else:
if not self.opPointModif.activ:
plug(self.opPointModif.signal('position'),
self.proj.signal('transfo'))
plug(self.opPointModif.signal('jacobian'),
self.feature.signal('Jq'))
self.opPointModif.setTransformation(m)
self.opPointModif.activ = True
def goto3D(self, point3D, gain=None, ref2D=None, selec=None):
self.target = point3D
if ref2D is not None:
self.ref = ref2D
if selec is not None:
self.feature.selec.value = selec
setGain(self.gain, gain)
robot.com_admittance_control = com_admittance_control
Kp_adm = [15.0, 15.0, 0.0] # this value is employed later
# --- Control Manager
robot.cm = create_ctrl_manager(cm_conf, dt, robot_name='robot')
robot.cm.addCtrlMode('sot_input')
robot.cm.setCtrlMode('all', 'sot_input')
robot.cm.addEmergencyStopSIN('zmp')
robot.cm.addEmergencyStopSIN('distribute')
# -------------------------- SOT CONTROL --------------------------
# --- Upper body
robot.taskUpperBody = Task('task_upper_body')
robot.taskUpperBody.feature = FeaturePosture('feature_upper_body')
q = list(robot.dynamic.position.value)
robot.taskUpperBody.feature.state.value = q
robot.taskUpperBody.feature.posture.value = q
robotDim = robot.dynamic.getDimension() # 38
for i in range(18, robotDim):
robot.taskUpperBody.feature.selectDof(i, True)
robot.taskUpperBody.controlGain.value = 100.0
robot.taskUpperBody.add(robot.taskUpperBody.feature.name)
plug(robot.dynamic.position, robot.taskUpperBody.feature.state)
# --- CONTACTS
def buildurrobot(self):
self.robot = Ur("ur5", device=RobotSimu("ur5"))
robot_dimension = self.robot.dynamic.getDimension()
self.robot.device.resize(robot_dimension)
# Ros binding
# roscore must be running
ros = Ros(self.robot)
# collision components
self.a = sc.SotCollision("sc")
# self.a.createlinkmodel(((0,0.08,0.01,0,0,0.0,0.0,0,0),(1,0.08,0.04,0,0,0,0,0,0),(2,0.09,0.15,0.2,0.0,0.0,0,1.57,0),(3,0.07,0.14,0.2,0.0,0.0,-3.1416,1.5706,-3.1416),(4,0.05,0.03,0.0,0.093,0,-3.14,0,-3.14),(5,0.057,0.02,0.0,0,-0.095,-3.14,0,-3.14),(6,0.04,0.01,0.0,0.065,0,1.57,0,0)))
# create links for collision check
self.a.createcollisionlink("base_link", "capsule", "internal", (0.08, 0.01, 0, 0, 0, 0.0, 0.0, 0, 0))
self.a.createcollisionlink("shoulder_pan_link", "capsule", "internal", (0.08, 0.04, 0, 0, 0, 0, 0, 0, 0))
self.a.createcollisionlink(
"shoulder_lift_link", "capsule", "internal", (0.09, 0.15, 0, 0.2, 0.0, 0.0, 0, 1.57, 0)
)
self.a.createcollisionlink(
"elbow_link", "capsule", "internal", (0.07, 0.14, 0, 0.2, 0.0, 0.0, -3.1416, 1.5706, -3.1416)
)
self.a.createcollisionlink(
"wrist_1_link", "capsule", "internal", (0.05, 0.03, 0, 0.0, 0.093, 0, -3.14, 0, -3.14)
)
self.a.createcollisionlink(
"wrist_2_link", "capsule", "internal", (0.057, 0.02, 0, 0.0, 0, -0.095, -3.14, 0, -3.14)
)
self.a.createcollisionlink("wrist_3_link", "capsule", "internal", (0.04, 0.01, 0, 0.0, 0.065, 0, 1.57, 0, 0))
# plug joint position and joint jacobian to collision checker entity
plug(self.robot.dynamic.base_joint, self.a.base_link)
plug(self.robot.dynamic.shoulder_pan_joint, self.a.shoulder_pan_link)
plug(self.robot.dynamic.shoulder_lift_joint, self.a.shoulder_lift_link)
plug(self.robot.dynamic.elbow_joint, self.a.elbow_link)
plug(self.robot.dynamic.wrist_1_joint, self.a.wrist_1_link)
plug(self.robot.dynamic.wrist_2_joint, self.a.wrist_2_link)
plug(self.robot.dynamic.wrist_3_joint, self.a.wrist_3_link)
plug(self.robot.dynamic.Jbase_joint, self.a.Jbase_link)
plug(self.robot.dynamic.Jshoulder_pan_joint, self.a.Jshoulder_pan_link)
plug(self.robot.dynamic.Jshoulder_lift_joint, self.a.Jshoulder_lift_link)
plug(self.robot.dynamic.Jelbow_joint, self.a.Jelbow_link)
plug(self.robot.dynamic.Jwrist_1_joint, self.a.Jwrist_1_link)
plug(self.robot.dynamic.Jwrist_2_joint, self.a.Jwrist_2_link)
plug(self.robot.dynamic.Jwrist_3_joint, self.a.Jwrist_3_link)
# create collision pairs
# base link
self.a.createcollisionpair("base_link", "shoulder_lift_link")
self.a.createcollisionpair("base_link", "elbow_link")
self.a.createcollisionpair("base_link", "wrist_1_link")
self.a.createcollisionpair("base_link", "wrist_2_link")
self.a.createcollisionpair("base_link", "wrist_3_link")
# shoulder pan link
self.a.createcollisionpair("shoulder_pan_link", "wrist_1_link")
self.a.createcollisionpair("shoulder_pan_link", "wrist_2_link")
self.a.createcollisionpair("shoulder_pan_link", "wrist_3_link")
self.a.createcollisionpair("shoulder_pan_link", "elbow_link")
# shoulder lift link
self.a.createcollisionpair("shoulder_lift_link", "wrist_1_link")
self.a.createcollisionpair("shoulder_lift_link", "wrist_2_link")
self.a.createcollisionpair("shoulder_lift_link", "wrist_3_link")
# shoulder elbow link
self.a.createcollisionpair("elbow_link", "wrist_2_link")
self.a.createcollisionpair("elbow_link", "wrist_3_link")
# shoulder wrist1 link
self.a.createcollisionpair("wrist_1_link", "wrist_3_link")
######### task description for old type of solver############
# Create task for the waist
robot_pose = ((1.0, 0, 0, 0), (0, 1.0, 0, 0), (0, 0, 1.0, 0), (0, 0, 0, 1.0))
feature_base = FeaturePosition(
"position_base", self.robot.dynamic.base_joint, self.robot.dynamic.Jbase_joint, robot_pose
)
task_base = Task("waist_task")
task_base.controlGain.value = 100.0
task_base.add(feature_base.name)
# Create task for the wrist3
I4 = ((1.0, 0, 0, 0.321), (0, 1.0, 0, 0.109), (0, 0, 1.0, 0.848), (0, 0, 0, 1.0))
feature_wrist = FeaturePosition(
"position_wrist", self.robot.dynamic.wrist_3_joint, self.robot.dynamic.Jwrist_3_joint, I4
)
task_wrist = Task("wrist_task")
task_wrist.controlGain.value = 1
task_wrist.add(feature_wrist.name)
######### task description for new type of solver############
# waist position task
goal = ((1.0, 0, 0, 0), (0, 1.0, 0, 0), (0, 0, 1.0, 0), (0, 0, 0, 1.0))
self.task_waist_metakine = MetaTaskKine6d("task_waist_metakine", self.robot.dynamic, "base_joint", "base_joint")
# task_waist_metakine.opmodif = goal
# task_waist_metakine.feature.frame('desired')
self.task_waist_metakine.featureDes.position.value = goal
self.task_waist_metakine.gain.setConstant(100)
# wrist_3 task
goal = ((1.0, 0, 0, 0.321), (0, 1.0, 0, 0.109), (0, 0, 1.0, 0.848), (0, 0, 0, 1.0))
self.task_wrist_metakine = MetaTaskKine6d(
"task_wrist_metakine", self.robot.dynamic, "wrist_3_joint", "wrist_3_joint"
)
# task_wrist_metakine.opmodif = goal
# task_wrist_metakine.feature.frame('desired')
self.task_wrist_metakine.featureDes.position.value = goal
self.task_wrist_metakine.gain.setConstant(1)
######### task description for collision task ############
self.task_collision_avoidance = TaskInequality("taskcollision")
self.collision_feature = FeatureGeneric("collisionfeature")
plug(self.a.collisionJacobian, self.collision_feature.jacobianIN)
plug(self.a.collisionDistance, self.collision_feature.errorIN)
# self.a.collisionDistance
self.task_collision_avoidance.add(self.collision_feature.name)
self.task_collision_avoidance.referenceInf.value = (0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, -1, 0, 0) # min
self.task_collision_avoidance.referenceSup.value = (
2e10,
2e10,
2e10,
2e10,
2e10,
2e10,
2e10,
2e10,
2e10,
2e10,
2e10,
2e10,
2e10,
2e10,
2e10,
) # max
self.task_collision_avoidance.dt.value = self.dt
self.task_collision_avoidance.controlGain.value = 0.01
# task.featureDes.xy.value = (0,0)
"""
#task_wrist_metakine.opmodif = goal
#task_wrist_metakine.feature.frame('desired')
task_wrist_metakine.featureDes.position.value = goal
task_wrist_metakine.gain.setConstant(1)
"""
######### solver###########################################################
##### solver old############
"""
self.solver = SOT ('solver')
self.solver.setSize (robot_dimension)
self.solver.push (task_base.name)
self.solver.push (task_wrist.name)
plug (self.solver.control, self.robot.device.control)
"""
self.solver = SolverKine("sot")
self.solver.setSize(robot_dimension)
self.solver.push(self.task_waist_metakine.task.name)
self.solver.push(self.task_collision_avoidance.name)
self.solver.push(self.task_wrist_metakine.task.name)
self.solver.damping.value = 3e-2
plug(self.solver.control, self.robot.device.control)