test track single joint

CMakeLists.txt

@@ -139,10 +139,12 @@ IF(BUILD_PYTHON_INTERFACE)
   INSTALL(FILES python/${SOTTALOSBALANCE_PYNAME}/__init__.py
                 python/${SOTTALOSBALANCE_PYNAME}/main.py
                 python/${SOTTALOSBALANCE_PYNAME}/create_entities_utils.py
+                python/${SOTTALOSBALANCE_PYNAME}/meta_task_config.py
                 DESTINATION ${PYTHON_SITELIB}/${SOTTALOSBALANCE_PYNAME})
   IF(BUILD_TEST)
     INSTALL(FILES python/${SOTTALOSBALANCE_PYNAME}/test/__init__.py
                   python/${SOTTALOSBALANCE_PYNAME}/test/run_test_utils.py
+                  python/${SOTTALOSBALANCE_PYNAME}/test/test_singleTraj.py
                   python/${SOTTALOSBALANCE_PYNAME}/test/test_jointTrajGen.py
                   python/${SOTTALOSBALANCE_PYNAME}/test/test_jointControl.py
                   python/${SOTTALOSBALANCE_PYNAME}/test/test_COMTraj.py

python/sot_talos_balance/create_entities_utils.py

@@ -12,6 +12,14 @@ def create_joint_trajectory_generator(dt):
     jtg.init(dt, N_JOINTS);
     return jtg;
 
+def create_config_trajectory_generator(dt):
+    N_CONFIG = N_JOINTS + 6
+    jtg = NdTrajectoryGenerator("jtg");
+    jtg.initial_value.value = tuple(N_CONFIG*[0.0]);
+    jtg.trigger.value = 1.0;
+    jtg.init(dt, N_CONFIG);
+    return jtg;
+
 def create_com_trajectory_generator(dt,robot):
     comTrajGen = NdTrajectoryGenerator("comTrajGen");
     comTrajGen.initial_value.value = robot.dynamic.com.value

python/sot_talos_balance/meta_task_config.py

+from dynamic_graph.sot.core.meta_task_6d import toFlags
+from dynamic_graph import plug
+from dynamic_graph.sot.core import *
+# from dynamic_graph.sot.core.meta_tasks import setGain
+# from dynamic_graph.sot.dyninv import *
+from dynamic_graph.sot.core.matrix_util import matrixToTuple, vectorToTuple,rotate
+from numpy import matrix, identity, zeros, eye
+
+
+class MetaTaskConfig(object):
+    nbDof = None
+
+    def __init__(self,dyn,config,name="joint"):
+        self.dyn=dyn
+        self.name=name
+        self.config = config
+
+        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)
+        self.feature.selec.value = toFlags(self.config)
+
+    def plugTask(self):
+        self.task.add(self.feature.name)
+        plug(self.task.error,self.gain.error)
+        plug(self.gain.gain,self.task.controlGain)
+
+    @property
+    def ref(self):
+        return self.featureDes.errorIN.value
+
+    @ref.setter
+    def ref(self,v):
+        self.featureDes.errorIN.value = v
+
+class MetaTaskKineConfig(MetaTaskConfig):
+    def __init__(self,dyn,config,name="joint"):
+        MetaTaskConfig.__init__(self,dyn,config,name)
+        self.task = Task('task'+name)
+        self.plugTask()

python/sot_talos_balance/test/test_jointControl.py

@@ -31,7 +31,7 @@ def main(robot,gain):
     sleep(1.0)
     robot.traj_gen.move(31,-1.0,1.0)
     sleep(1.1)
-    robot.traj_gen.startSinusoid(31,3.0,2.0)
+    robot.traj_gen.startSinusoid(31,1.0,2.0)
     sleep(10.0)
     robot.traj_gen.stop(31)
 

python/sot_talos_balance/test/test_jointTrajGen.py

@@ -17,6 +17,6 @@ def main(robot):
     sleep(1.0);
     robot.traj_gen.move(31,-1.0,1.0);
     sleep(1.1);
-    robot.traj_gen.startSinusoid(31,3.0,2.0);
+    robot.traj_gen.startSinusoid(31,1.0,2.0);
     sleep(10.0);
     robot.traj_gen.stop(31);

python/sot_talos_balance/test/test_singleTraj.py

+from sot_talos_balance.create_entities_utils import create_config_trajectory_generator
+from dynamic_graph.sot.core.meta_tasks_kine import MetaTaskKine6d
+from sot_talos_balance.meta_task_config import MetaTaskKineConfig
+from dynamic_graph.sot.core.matrix_util import matrixToTuple
+from dynamic_graph import plug
+from dynamic_graph.sot.core import SOT
+from numpy import eye
+from time import sleep
+import os
+
+N_JOINTS = 32
+N_CONFIG = N_JOINTS + 6
+
+def main(robot):
+    dt = robot.timeStep;
+    robot.traj_gen = create_config_trajectory_generator(dt)
+
+    JOINT = 31
+    QJOINT = JOINT + 6
+
+    # --- Joint
+    robot.taskJoint = MetaTaskKineConfig(robot.dynamic,[QJOINT])
+    # robot.dynamic.com.recompute(0)
+    robot.taskJoint.featureDes.errorIN.value = N_CONFIG*[0.0]
+    robot.taskJoint.task.controlGain.value = 100
+
+    # --- CONTACTS
+    #define contactLF and contactRF
+    robot.contactLF = MetaTaskKine6d('contactLF',robot.dynamic,'LF',robot.OperationalPointsMap['left-ankle'])
+    robot.contactLF.feature.frame('desired')
+    robot.contactLF.gain.setConstant(100)
+    robot.contactLF.keep()
+    locals()['contactLF'] = robot.contactLF
+
+    robot.contactRF = MetaTaskKine6d('contactRF',robot.dynamic,'RF',robot.OperationalPointsMap['right-ankle'])
+    robot.contactRF.feature.frame('desired')
+    robot.contactRF.gain.setConstant(100)
+    robot.contactRF.keep()
+    locals()['contactRF'] = robot.contactRF
+
+    robot.sot = SOT('sot')
+    robot.sot.setSize(robot.dynamic.getDimension())
+    plug(robot.sot.control,robot.device.control)
+
+    robot.sot.push(robot.contactRF.task.name)
+    robot.sot.push(robot.contactLF.task.name)
+    robot.sot.push(robot.taskJoint.task.name)
+    robot.device.control.recompute(0)
+
+    plug(robot.traj_gen.x,    robot.taskJoint.featureDes.errorIN)
+    sleep(1.0)
+    os.system('rosservice call /start_dynamic_graph')
+    sleep(1.0)
+    robot.traj_gen.move(QJOINT,-1.0,1.0)
+    sleep(1.1);
+    robot.traj_gen.startSinusoid(QJOINT,1.0,8.0)
+    sleep(10.0)
+    robot.traj_gen.stop(QJOINT)
+