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