Update to new API.

Conflicts:

	unitTesting/python/dynamic_walk.py
	unitTesting/python/feet_follower.py

AUTHORS

@@ -2,6 +2,6 @@ This package was written by and with the assistance of:
 
 * Francois Bleibel	fbleibel@gmail.com
 * François Keith	francois.keith@aist.go.jp 
-* Nicolas Mansard	FIXME
+* Nicolas Mansard       nicolas.mansard@laas.fr	
 * Olivier Stasse	olivier.stasse@aist.go.jp
 * Thomas Moulard        thomas.moulard@gmail.com

unitTesting/python/dynamic_walk.py

+#!/usr/bin/env python
+# -*- coding: utf-8 -*-
+# Copyright 2011, Florent Lamiraux, Thomas Moulard, JRL, CNRS/AIST
+#
+# This file is part of dynamic-graph.
+# dynamic-graph is free software: you can redistribute it and/or
+# modify it under the terms of the GNU Lesser General Public License
+# as published by the Free Software Foundation, either version 3 of
+# the License, or (at your option) any later version.
+#
+# dynamic-graph is distributed in the hope that it will be useful, but
+# WITHOUT ANY WARRANTY; without even the implied warranty of
+# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
+# General Lesser Public License for more details.  You should have
+# received a copy of the GNU Lesser General Public License along with
+# dynamic-graph. If not, see <http://www.gnu.org/licenses/>.
+
+from math import pi
+
+from dynamic_graph.sot.core import FeaturePoint6d, FeatureGeneric, SOT
+from dynamic_graph.sot.se3 import SE3
+from dynamic_graph.sot.dynamics.humanoid_robot import HumanoidRobot
+from dynamic_graph.sot.dynamics.hrp2 import Hrp2
+
+from dynamic_graph import enableTrace, plug
+
+from tools import *
+
+
+class HalfStep:
+    startX = 0.
+    startY = 0.
+    # startZ is always 0.
+    startTheta = 0.
+
+    # finalX is always 0.
+    finalY = 0.
+    finalZ = 0.
+    # finalTheta is always 0.
+
+class DynamicWalking:
+    leftFoot = 0
+    rightFoot = 1
+
+    footAltitude = 0.1
+
+    robot = None
+    supportFoot = None
+
+    swingFoot = None
+
+    def __init__(self, robot):
+        self.robot = robot
+        self.supportFoot = self.leftFoot
+
+        self.swingFoot = HalfStep()
+        self.swingFoot.startX = -0.1
+        self.swingFoot.startY =  0.1
+        self.swingFoot.startTheta = pi / 4.
+
+        self.swingFoot.finalY = 0.1
+        self.swingFoot.finalZ = 0.
+
+    def computeFootPosition(op, swingFoot):
+        # Support foot position in the waist frame.
+        supportFootPos = self.robot.dynamicRobot.signal(op).value
+        # Swing foot in the support foot: swingFoot
+
+        # Swing foot in the waist frame.
+        return SE3(supportFootPos) * SE3(swingFoot)
+
+res = \
+    SE3(robot.features['right-ankle'].reference.position.value) * \
+    SE3((0., 0., 0., -0.1),
+        (0., 0., 0.,  0.1),
+        (0., 0., 0.,  0.),
+        (0., 0., 0.,  1.))
+print res
+
+
+# Push tasks
+#  Feet tasks.
+solver.sot.push(robot.name + '.task.right-ankle')
+solver.sot.push(robot.name + '.task.left-ankle')
+
+#  Center of mass
+# FIXME: trigger segv at exit.
+solver.sot.push(robot.name + '.task.com')
+
+dynamicWalking = DynamicWalking(robot)
+
+for i in xrange(100):
+    robot.simu.increment(timeStep)
+
+    if clt:
+        clt.updateElementConfig(
+            'hrp', robot.smallToFull(robot.simu.state.value))

unitTesting/python/feet_follower.py

+#!/usr/bin/env python
+# -*- coding: utf-8 -*-
+# Copyright 2011, Florent Lamiraux, Thomas Moulard, JRL, CNRS/AIST
+#
+# This file is part of dynamic-graph.
+# dynamic-graph is free software: you can redistribute it and/or
+# modify it under the terms of the GNU Lesser General Public License
+# as published by the Free Software Foundation, either version 3 of
+# the License, or (at your option) any later version.
+#
+# dynamic-graph is distributed in the hope that it will be useful, but
+# WITHOUT ANY WARRANTY; without even the implied warranty of
+# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
+# General Lesser Public License for more details.  You should have
+# received a copy of the GNU Lesser General Public License along with
+# dynamic-graph. If not, see <http://www.gnu.org/licenses/>.
+
+from tools import *
+
+from dynamic_graph.sot.dynamics.feet_follower import FeetFollowerFromFile
+feetFollower = FeetFollowerFromFile('feet-follower')
+
+feetFollower.feetToAnkleLeft = robot.dynamic.getAnklePositionInFootFrame()
+feetFollower.feetToAnkleRight = robot.dynamic.getAnklePositionInFootFrame()
+
+plug(feetFollower.signal('com'), robot.featureComDes.signal('errorIN'))
+plug(feetFollower.signal('left-ankle'),
+     robot.features['left-ankle'].reference)
+plug(feetFollower.signal('right-ankle'),
+     robot.features['right-ankle'].reference)
+
+robot.comTask.signal('controlGain').value = 50.
+robot.tasks['left-ankle'].signal('controlGain').value = 50.
+robot.tasks['right-ankle'].signal('controlGain').value = 50.
+
+# Push tasks
+#  Operational points tasks
+solver.sot.push(robot.name + '.task.right-ankle')
+solver.sot.push(robot.name + '.task.left-ankle')
+
+#  Center of mass
+solver.sot.push(robot.name + '.task.com')
+
+# Main.
+#  Main loop
+for i in xrange(500):
+    robot.simu.increment(timeStep)
+
+    if clt:
+        clt.updateElementConfig(
+            'hrp', robot.smallToFull(robot.simu.state.value))
+
+
+finalPosition = (
+    -0.015361, -0.0049075500000000001, -0.00047065200000000001, -0.0172946,
+     -0.020661800000000001, 0.0374547, -0.037641599999999997,
+     0.025434399999999999, -0.45398100000000002, 0.86741800000000002,
+     -0.39213799999999999, -0.0089269499999999995, -0.037646100000000002,
+     0.025648199999999999, -0.46715499999999999, 0.87717599999999996,
+     -0.38872200000000001, -0.0091408199999999992, 0.080488199999999996,
+     -0.18355399999999999, -0.00036695100000000002, -0.0056776600000000002,
+     -0.12173299999999999, -0.23972599999999999, -0.00637303,
+     -0.56908000000000003, 0.00296262, 0.19108900000000001, 0.100088,
+     0.23896800000000001, 0.21485599999999999, -0.18973400000000001,
+     -0.49457699999999999, 0.040646799999999997, 0.16970299999999999, 0.100067)
+
+checkFinalConfiguration(robot.simu.state.value, finalPosition)
+print "Exiting."

unitTesting/python/quasistatic_walking.py

@@ -57,14 +57,14 @@ class QuasiStaticWalking:
         self.nextStateSwitch = 0 # Next switch is now!
 
         self.initialFootPose['left-ankle'] = \
-            self.robot.dynamicRobot.signal('left-ankle').value
+            self.robot.dynamic.signal('left-ankle').value
         self.initialFootPose['right-ankle'] = \
-            self.robot.dynamicRobot.signal('right-ankle').value
+            self.robot.dynamic.signal('right-ankle').value
 
         self.t = None # Will be updated through the update method.
 
-        self.robot.tasks['left-ankle'].signal('controlGain').value = 1.
-        self.robot.tasks['right-ankle'].signal('controlGain').value = 1.
+        self.robot.tasks['left-ankle'].controlGain.value = 1.
+        self.robot.tasks['right-ankle'].controlGain.value = 1.
 
 
     # Move CoM to a particular operational point (usually left or right ankle).
@@ -77,19 +77,19 @@ class QuasiStaticWalking:
             x = 0.
             y = 0.
         else:
-            x = robot.dynamicRobot.signal(op).value[0][3]
-            y = robot.dynamicRobot.signal(op).value[1][3]
+            x = robot.dynamic.signal(op).value[0][3]
+            y = robot.dynamic.signal(op).value[1][3]
 
-        z = robot.featureComDes.signal('errorIN').value[2]
-        self.robot.featureComDes.signal('errorIN').value = (x, y, z)
+        z = robot.featureComDes.errorIN.value[2]
+        self.robot.featureComDes.errorIN.value = (x, y, z)
 
     def liftFoot(self, op):
-        sdes = toList(robot.dynamicRobot.signal(op).value)
+        sdes = toList(robot.dynamic.signal(op).value)
         sdes[2][3] += self.footAltitude # Increment altitude.
-        robot.features[op].reference.signal('position').value = toTuple(sdes)
+        robot.features[op].reference.value = toTuple(sdes)
 
     def landFoot(self, op):
-        robot.features[op].reference.signal('position').value = \
+        robot.features[op].reference.value = \
             self.initialFootPose[op]
 
     def supportFootStr(self):
@@ -180,13 +180,13 @@ totalSteps = int((stepTime / timeStep) * steps)
 t = 0
 for i in xrange(totalSteps + 1):
     t += timeStep
-    robot.robotSimu.increment(timeStep)
+    robot.simu.increment(timeStep)
 
     quasiStaticWalking.update(t)
 
     if clt:
         clt.updateElementConfig(
-            'hrp', robot.smallToFull(robot.robotSimu.signal('state').value))
+            'hrp', robot.smallToFull(robot.simu.state.value))
 
 #  Security: switch back to double support.
 quasiStaticWalking.moveCoM('origin')
@@ -194,11 +194,11 @@ duration = quasiStaticWalking.time[quasiStaticWalking.stateCoM_singleToDouble]
 
 for i in xrange(int(duration / timeStep)):
     t += timeStep
-    robot.robotSimu.increment(timeStep)
+    robot.simu.increment(timeStep)
 
     if clt:
         clt.updateElementConfig(
-            'hrp', robot.smallToFull(robot.robotSimu.signal('state').value))
+            'hrp', robot.smallToFull(robot.simu.state.value))
 
 finalPosition = (
     -0.0082169200000000008, -0.0126068, -0.00022860999999999999,
@@ -213,5 +213,5 @@ finalPosition = (
      0.26377400000000001, 0.171155, -0.00065098499999999998,
      -0.52324700000000002, -1.23291e-05, 6.0469500000000001e-05, 0.100009)
 
-checkFinalConfiguration(robot.robotSimu.signal('state').value, finalPosition)
+checkFinalConfiguration(robot.simu.state.value, finalPosition)
 print "Exiting."

unitTesting/python/reach_both_hands.py

@@ -35,11 +35,11 @@ solver.sot.push(robot.name + '.task.com')
 # Main.
 #  Main loop
 for i in xrange(500):
-    robot.robotSimu.increment(timeStep)
+    robot.simu.increment(timeStep)
 
     if clt:
         clt.updateElementConfig(
-            'hrp', robot.smallToFull(robot.robotSimu.signal('state').value))
+            'hrp', robot.smallToFull(robot.simu.state.value))
 
 finalPosition = (
     -0.0357296, -0.0024092699999999998, 0.033870600000000001,
@@ -55,5 +55,5 @@ finalPosition = (
      -0.109959, -0.60156299999999996, -0.082422700000000002,
      1.0207200000000001, 0.10037500000000001)
 
-checkFinalConfiguration(robot.robotSimu.signal('state').value, finalPosition)
+checkFinalConfiguration(robot.simu.state.value, finalPosition)
 print "Exiting."

unitTesting/python/reach_left_hand.py

@@ -34,11 +34,11 @@ solver.sot.push(robot.name + '.task.com')
 # Main.
 #  Main loop
 for i in xrange(500):
-    robot.robotSimu.increment(timeStep)
+    robot.simu.increment(timeStep)
 
     if clt:
         clt.updateElementConfig(
-            'hrp', robot.smallToFull(robot.robotSimu.signal('state').value))
+            'hrp', robot.smallToFull(robot.simu.state.value))
 
 finalPosition = (
     -0.015183500000000001, -0.00037148200000000002, -0.00065935600000000005,
@@ -53,5 +53,5 @@ finalPosition = (
      0.170987, 0.100064, -0.117492, 0.24870200000000001, 0.016264399999999998,
      -0.56795700000000005, 0.0040012399999999997, 0.18956200000000001, 0.100089)
 
-checkFinalConfiguration(robot.robotSimu.signal('state').value, finalPosition)
+checkFinalConfiguration(robot.simu.state.value, finalPosition)
 print "Exiting."

unitTesting/python/reach_right_hand.py

@@ -33,11 +33,11 @@ solver.sot.push(robot.name + '.task.com')
 # Main.
 #  Main loop
 for i in xrange(500):
-    robot.robotSimu.increment(timeStep)
+    robot.simu.increment(timeStep)
 
     if clt:
         clt.updateElementConfig(
-            'hrp', robot.smallToFull(robot.robotSimu.signal('state').value))
+            'hrp', robot.smallToFull(robot.simu.state.value))
 
 
 finalPosition = (
@@ -53,5 +53,5 @@ finalPosition = (
      0.23896800000000001, 0.21485599999999999, -0.18973400000000001,
      -0.49457699999999999, 0.040646799999999997, 0.16970299999999999, 0.100067)
 
-checkFinalConfiguration(robot.robotSimu.signal('state').value, finalPosition)
+checkFinalConfiguration(robot.simu.state.value, finalPosition)
 print "Exiting."

unitTesting/python/tools.py

@@ -114,11 +114,11 @@ def initRobotViewer():
     return clt
 
 def reach(robot, op, tx, ty, tz):
-    sdes = toList(robot.dynamicRobot.signal(op).value)
+    sdes = toList(robot.dynamic.signal(op).value)
     sdes[0][3] += tx
     sdes[1][3] += ty
     sdes[2][3] += tz
-    robot.features[op].reference.signal('position').value = toTuple(sdes)
+    robot.features[op].reference.value = toTuple(sdes)
     # Select translation only.
     robot.features[op].feature.signal('selec').value = '000111'
     robot.tasks[op].signal('controlGain').value = 1.
@@ -154,10 +154,10 @@ class Solver:
         self.sot.signal('damping').value = 1e-6
         self.sot.setNumberDofs(self.robot.dimension)
 
-        if robot.robotSimu:
-            plug(self.sot.signal('control'), robot.robotSimu.signal('control'))
-            plug(self.robot.robotSimu.signal('state'),
-                 self.robot.dynamicRobot.signal('position'))
+        if robot.simu:
+            plug(self.sot.signal('control'), robot.simu.signal('control'))
+            plug(self.robot.simu.state,
+                 self.robot.dynamic.position)
 
 
 ##################
@@ -182,5 +182,5 @@ if options.display:
 
 # Initialize the stack of tasks.
 robot = Hrp2("robot", True)
-timeStep = .02
+timeStep = .005
 solver = Solver(robot)