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
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
