def initPostureTask(robot):
# --- TASK POSTURE --------------------------------------------------
# set a default position for the joints.
robot.features['featurePosition'] = FeaturePosture('featurePosition')
plug(robot.device.state, robot.features['featurePosition'].state)
robot.features['featurePosition'].posture.value = robot.halfSitting
# Remove the dofs of the feet.
postureTaskDofs = [True] * (len(robot.dynamic.position.value) - 6)
jla = robot.dynamic.signal('J' +
robot.OperationalPointsMap['left-ankle']).value
postureTaskDofs = removeDofUsed(jla, postureTaskDofs)
jra = robot.dynamic.signal('J' +
robot.OperationalPointsMap['right-ankle']).value
postureTaskDofs = removeDofUsed(jra, postureTaskDofs)
for dof, isEnabled in enumerate(postureTaskDofs):
robot.features['featurePosition'].selectDof(dof + 6, isEnabled)
robot.tasks['robot_task_position'] = Task('robot_task_position')
robot.tasks['robot_task_position'].add('featurePosition')
gainPosition = GainAdaptive('gainPosition')
gainPosition.set(0.1, 0.1, 125e3)
gainPosition.gain.value = 5
plug(robot.tasks['robot_task_position'].error, gainPosition.error)
plug(gainPosition.gain, robot.tasks['robot_task_position'].controlGain)
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 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 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 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 __init__(self, dyn, name="posture"):
self.dyn = dyn
self.name = name
self.feature = FeatureGeneric('feature' + name)
self.featureDes = FeatureGeneric('featureDes' + name)
self.gain = GainAdaptive('gain' + name)
plug(dyn.position, self.feature.errorIN)
robotDim = len(dyn.position.value)
self.feature.jacobianIN.value = matrixToTuple(identity(robotDim))
self.feature.setReference(self.featureDes.name)
def __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, name, sotrobot, withDerivative=False):
super(Posture, self).__init__()
n = self._name(name)
self._task = SotTask(n + '_task')
self._task.dyn = sotrobot.dynamic
self._feature = FeaturePosture(n + '_feature_')
q = list(sotrobot.dynamic.position.value)
self._feature.state.value = q
self._feature.posture.value = q
robotDim = sotrobot.dynamic.getDimension()
for i in range(6, robotDim):
self._feature.selectDof(i, True)
self._gain = GainAdaptive(n + "_gain")
self._task.add(self._feature.name)
# Connects the dynamics to the current feature of the posture task
plug(sotrobot.dynamic.position, self._feature.state)
self._task.setWithDerivative(withDerivative)
# Set the gain of the posture task
setGain(self._gain, (4.9, 0.9, 0.01, 0.9))
plug(self._gain.gain, self._task.controlGain)
plug(self._task.error, self._gain.error)
self.tasks = [self._task]
self.topics = {
name: {
"type": "vector",
"topic": "/hpp/target/position",
"signalGetters": frozenset([self._signalPositionRef])
},
}
if withDerivative:
self.topics["vel_" + name] = {
"type": "vector",
"topic": "/hpp/target/velocity",
"signalGetters": frozenset([self._signalVelocityRef])
}
def createGain(self):
self.gain = GainAdaptive('gain' + self.name)
self.gain.set(0.1, 0.1, 125e3)
class MetaTask6d(object):
name = ''
opPoint = ''
dyn = 0
task = 0
feature = 0
featureDes = 0
def opPointExist(self, opPoint):
sigsP = [x for x in self.dyn.signals() if x.getName().split(':')[-1] == opPoint]
sigsJ = [x for x in self.dyn.signals() if x.getName().split(':')[-1] == 'J' + opPoint]
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 = Flags('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
from dynamic_graph.sot.core.gain_adaptive import GainAdaptive
from dynamic_graph.sot.core.matrix_util import matrixToTuple
from dynamic_graph.sot.core.meta_tasks import setGain
from dynamic_graph.sot.core.operator import Selec_of_vector
# Create the solver
# Connects the sequence player to the posture task
from dynamic_graph.sot.core.sot import SOT, Task
from dynamic_graph.sot.tools import SimpleSeqPlay
# 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)
plug(aSimpleSeqPlay.posture, taskPosture.featureDes.errorIN)
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)
t = VectorConstant('t')
# WAIST
t.set((0., 0.095, 0.563816))
# HAND
t.set((0.25, -0.5, .85))
plug(t.signal('out'), comp.signal('sin2'))
plug(comp.signal('sout'), p6d.signal('position'))
plug(dyn.signal('J0'), p6.signal('Jq'))
plug(dyn.signal('0'), p6.signal('position'))
p6.signal('sdes').value = p6d
task = Task('task')
task.add('p6')
gain = GainAdaptive('gain')
gain.setConstant(.2)
plug(task.signal('error'), gain.signal('error'))
plug(gain.signal('gain'), task.signal('controlGain'))
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
t = VectorConstant('t')
# WAIST
t.set((0., 0.095, 0.563816))
# HAND
t.set((0.25, -0.5, .85))
plug(t.signal('out'), comp.signal('in2'))
plug(comp.signal('out'), p6d.signal('position'))
plug(dyn.signal('J0'), p6.signal('Jq'))
plug(dyn.signal('0'), p6.signal('position'))
p6.signal('sdes').value = p6d
task = Task('task')
task.add('p6')
gain = GainAdaptive('gain')
gain.setConstant(.2)
plug(task.signal('error'), gain.signal('error'))
plug(gain.signal('gain'), task.signal('controlGain'))
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