diff --git a/CMakeLists.txt b/CMakeLists.txt
index 5a1aeb69ac6d4b2aa1c30a6bdd6209f57af1e815..f1efc417b7f172a00e86dc542811a9f65aa45f78 100644
--- a/CMakeLists.txt
+++ b/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
diff --git a/python/sot_talos_balance/create_entities_utils.py b/python/sot_talos_balance/create_entities_utils.py
index 0aee2ce7cce3513591f7a6f644715aa6e4e82469..3cf42991e0ff928e00cdb17436e79a3d9545e637 100644
--- a/python/sot_talos_balance/create_entities_utils.py
+++ b/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
diff --git a/python/sot_talos_balance/meta_task_config.py b/python/sot_talos_balance/meta_task_config.py
new file mode 100644
index 0000000000000000000000000000000000000000..3dc395412230a473393b4c8d9a999ee7a9906328
--- /dev/null
+++ b/python/sot_talos_balance/meta_task_config.py
@@ -0,0 +1,45 @@
+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()
diff --git a/python/sot_talos_balance/test/test_jointControl.py b/python/sot_talos_balance/test/test_jointControl.py
index e919c8a5599df0cbd1e19e34ab2d422d863e01a0..156443fa7c19d740be83609d3efe76806b218d13 100644
--- a/python/sot_talos_balance/test/test_jointControl.py
+++ b/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)
diff --git a/python/sot_talos_balance/test/test_jointTrajGen.py b/python/sot_talos_balance/test/test_jointTrajGen.py
index 5c370352e83d970bb1a1df4ad4ddbe24a0a3f704..3bc764a24edc0488cd5239de08dfbf9a5ebc2eab 100644
--- a/python/sot_talos_balance/test/test_jointTrajGen.py
+++ b/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);
diff --git a/python/sot_talos_balance/test/test_singleTraj.py b/python/sot_talos_balance/test/test_singleTraj.py
new file mode 100644
index 0000000000000000000000000000000000000000..6fb07d9d2ed51adb8fa70b6ee3a7e8035111ae7b
--- /dev/null
+++ b/python/sot_talos_balance/test/test_singleTraj.py
@@ -0,0 +1,59 @@
+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)
+