From f37edaab12ba3030a9a568c9c222a179f5dfdec9 Mon Sep 17 00:00:00 2001
From: Francois Keith <keith@lirmm.fr>
Date: Sat, 25 May 2013 23:58:59 +0200
Subject: [PATCH] Complete ros-walkromeo.py script
Remove useseless dependency in sot-dyninv.
Add the two missing tasks.
---
python/ros/ros-walkromeo.py | 24 +++++++++++++++++++++---
1 file changed, 21 insertions(+), 3 deletions(-)
diff --git a/python/ros/ros-walkromeo.py b/python/ros/ros-walkromeo.py
index 3b76c4d..4694294 100644
--- a/python/ros/ros-walkromeo.py
+++ b/python/ros/ros-walkromeo.py
@@ -11,7 +11,6 @@ from dynamic_graph.sot.core.matrix_util import matrixToTuple, vectorToTuple,rota
from dynamic_graph.sot.core.meta_tasks_kine import *
from dynamic_graph.sot.core.utils.thread_interruptible_loop import loopInThread,loopShortcuts
from dynamic_graph.sot.core.utils.viewer_helper import addRobotViewer,VisualPinger,updateComDisplay
-from dynamic_graph.sot.dyninv import SolverKine
from numpy import *
def totuple( a ):
@@ -90,11 +89,13 @@ taskLF.task.controlGain.value = 40
taskHead=MetaTask6d('head',robot.dynamic,'gaze','gaze')
# 2\ Link the orientation of the right foot to the desired position of the head.
+plug(taskRF.featureDes.position, taskHead.featureDes.position)
# 3\ Only constraint the rotation of the head.
+taskHead.feature.selec.value = '111000'
# 4\ set the gain
-
+taskHead.task.controlGain.value = 5
# ---- ARMS ---
@@ -105,10 +106,24 @@ taskHead=MetaTask6d('head',robot.dynamic,'gaze','gaze')
# * the reference position is the position given by the entity dyn,
# * the jacobian is the identity: ... = totuple( identity(robot.dimension) )
# * the joint controlled are those of the two arms [12,25] sur 39.
+featurePosition = FeatureGeneric('featurePosition')
+featurePositionDes = FeatureGeneric('featurePositionDes')
+featurePosition.setReference('featurePositionDes')
+plug(robot.dynamic.position,featurePosition.errorIN)
+featurePositionDes.errorIN.value = robot.halfSitting
+featurePosition.jacobianIN.value = totuple( identity(robot.dimension) )
# 2\ Define the task. Associate to the task the position feature.
+taskPosition = Task('taskPosition')
+taskPosition.add('featurePosition')
# 3\ (Optional) attach an adaptive gain (entity GainAdaptive) to the task created.
+gainPosition = GainAdaptive('gainPosition')
+gainPosition.set(0.1,0.1,125e3)
+gainPosition.gain.value = 5
+plug(taskPosition.error,gainPosition.error)
+plug(gainPosition.gain,taskPosition.controlGain)
+featurePosition.selec.value = '000000000000001111111111111100000000000'
############################################################################
@@ -121,7 +136,10 @@ solver.push(taskWaist.task)
solver.push(taskRF.task)
solver.push(taskLF.task)
solver.push(taskCom.task)
-# Activate your new tasks.
+# Stun the upper part of the body.
+sot.push(taskHead.task.name) # constraint the head orientation: look straight ahead
+sot.push('taskPosition') # stun the arms.
+
# --- HERDT PG AND START -------------------------------------------------------
# Set the algorithm generating the ZMP reference trajectory to Herdt's one.
--
GitLab