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)
# Connects the sequence player to the posture task
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)
# Set the gain of the posture task
setGain(tp.gain, (4.9, 0.9, 0.01, 0.9))
plug(tp.gain.gain, tp.controlGain)
plug(tp.error, tp.gain.error)
def getJointIdxQ(name):
model = robot.pinocchioModel
jid = model.getJointId(name)
return model.joints[jid].idx_q - 1
