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