Check final configurations in examples.

unitTesting/python/quasistatic_walking.py

@@ -200,5 +200,18 @@ for i in xrange(int(duration / timeStep)):
         clt.updateElementConfig(
             'hrp', robot.smallToFull(robot.robotSimu.signal('state').value))
 
-
-print "FINISHED"
+finalPosition = (
+    -0.0082169200000000008, -0.0126068, -0.00022860999999999999,
+     0.019962199999999999, -0.0159528, -0.00037375100000000002,
+     4.2206800000000002e-07, 0.0032752800000000002, -0.46070899999999998,
+     0.88794600000000001, -0.41127999999999998, -0.023234500000000002,
+     1.7543699999999999e-06, 0.00319733, -0.44445400000000002,
+     0.85594800000000004, -0.39553700000000003, -0.0231565, 0.00126246,
+     -0.0010669500000000001, -3.2293899999999999e-05, -0.00016289599999999999,
+     0.26377099999999998, -0.178532, -0.00082797999999999997,
+     -0.52297099999999996, -9.2273200000000003e-06, 0.000114984, 0.100008,
+     0.26377400000000001, 0.171155, -0.00065098499999999998,
+     -0.52324700000000002, -1.23291e-05, 6.0469500000000001e-05, 0.100009)
+
+checkFinalConfiguration(robot.robotSimu.signal('state').value, finalPosition)
+print "Exiting."

unitTesting/python/reach_both_hands.py

@@ -15,6 +15,7 @@
 # received a copy of the GNU Lesser General Public License along with
 # dynamic-graph. If not, see <http://www.gnu.org/licenses/>.
 
+import sys
 from tools import *
 
 # Move left wrist
@@ -40,4 +41,19 @@ for i in xrange(500):
         clt.updateElementConfig(
             'hrp', robot.smallToFull(robot.robotSimu.signal('state').value))
 
-print "FINISHED"
+finalPosition = (
+    -0.0357296, -0.0024092699999999998, 0.033870600000000001,
+     -0.00120006, 0.075338500000000003, 0.00028531699999999999,
+     2.9108999999999999e-05, 0.0053813699999999999, -0.41716799999999998,
+     0.52743700000000004, -0.185608, -0.0041716399999999999,
+     2.9120099999999999e-05, 0.0053815199999999999, -0.41625699999999999,
+     0.52549900000000005, -0.184581, -0.0041717899999999999, -0.00306069,
+     -0.54035500000000003, 0.00036732799999999999, -0.028043100000000001,
+     -0.69961799999999996, -0.84530499999999997, 0.11480700000000001,
+     -0.61734999999999995, 0.088004899999999997, 1.0022899999999999,
+     0.100354, -0.71066200000000002, 0.85328800000000005,
+     -0.109959, -0.60156299999999996, -0.082422700000000002,
+     1.0207200000000001, 0.10037500000000001)
+
+checkFinalConfiguration(robot.robotSimu.signal('state').value, finalPosition)
+print "Exiting."

unitTesting/python/reach_left_hand.py

@@ -15,6 +15,7 @@
 # received a copy of the GNU Lesser General Public License along with
 # dynamic-graph. If not, see <http://www.gnu.org/licenses/>.
 
+import sys
 from tools import *
 
 # Move left wrist
@@ -39,4 +40,18 @@ for i in xrange(500):
         clt.updateElementConfig(
             'hrp', robot.smallToFull(robot.robotSimu.signal('state').value))
 
-print "FINISHED"
+finalPosition = (
+    -0.015183500000000001, -0.00037148200000000002, -0.00065935600000000005,
+     0.0137784, -0.022388600000000002, -0.036052000000000001,
+     0.036032099999999997, -0.012359200000000001, -0.466526,
+     0.87994899999999998, -0.39055299999999998, -0.00060408700000000001,
+     0.036028499999999998, -0.0121996, -0.45257900000000001,
+     0.86809899999999995, -0.39265, -0.00076379799999999999,
+     -0.084612699999999999, -0.18716099999999999, 0.00038832500000000002,
+     -0.0054475900000000004, 0.23852599999999999, -0.19830900000000001,
+     0.17092299999999999, -0.48823699999999998, -0.014739800000000001,
+     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)
+print "Exiting."

unitTesting/python/reach_right_hand.py

@@ -39,4 +39,19 @@ for i in xrange(500):
         clt.updateElementConfig(
             'hrp', robot.smallToFull(robot.robotSimu.signal('state').value))
 
-print "FINISHED"
+
+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.robotSimu.signal('state').value, finalPosition)
+print "Exiting."

unitTesting/python/tools.py

@@ -15,6 +15,7 @@
 # received a copy of the GNU Lesser General Public License along with
 # dynamic-graph. If not, see <http://www.gnu.org/licenses/>.
 
+import sys
 from optparse import OptionParser
 
 from dynamic_graph import plug
@@ -72,6 +73,36 @@ def displayHomogeneousMatrix(matrix):
             formatStr += '\n'
     print formatStr.format(matrix_tuple)
 
+def displayHrp2Configuration(cfg):
+    if len(cfg) != 36:
+        raise "bad configuration size"
+    str = ''
+    str += 'Free flyer:\n'
+    str += ' translation {0[0]: <+10f} {0[1]: <+10f} {0[2]: <+10f}\n'
+    str += ' rotation    {0[3]: <+10f} {0[4]: <+10f} {0[5]: <+10f}\n'
+    str += 'Left leg:\n'
+    str += ' hip         {0[6]: <+10f} {0[7]: <+10f} {0[8]: <+10f}\n'
+    str += ' knee        {0[9]: <+10f}\n'
+    str += ' ankle       {0[10]: <+10f} {0[11]: <+10f}\n'
+    str += 'Right leg:\n'
+    str += ' hip         {0[12]: <+10f} {0[13]: <+10f} {0[14]: <+10f}\n'
+    str += ' knee        {0[15]: <+10f}\n'
+    str += ' ankle       {0[16]: <+10f} {0[17]: <+10f}\n'
+    str += 'Chest:       {0[18]: <+10f} {0[19]: <+10f}\n'
+    str += 'Head:        {0[20]: <+10f} {0[21]: <+10f}\n'
+    str += 'Left arm:\n'
+    str += ' shoulder    {0[22]: <+10f} {0[23]: <+10f} {0[24]: <+10f}\n'
+    str += ' elbow       {0[25]: <+10f}\n'
+    str += ' wrist       {0[26]: <+10f} {0[27]: <+10f}\n'
+    str += ' clench      {0[28]: <+10f}\n'
+    str += 'Left arm:\n'
+    str += ' shoulder    {0[29]: <+10f} {0[30]: <+10f} {0[31]: <+10f}\n'
+    str += ' elbow       {0[32]: <+10f}\n'
+    str += ' wrist       {0[33]: <+10f} {0[34]: <+10f}\n'
+    str += ' clench      {0[35]: <+10f}\n'
+    print str.format(cfg)
+
+
 def initRobotViewer():
     """Initialize robotviewer is possible."""
     clt = None
@@ -92,6 +123,22 @@ def reach(robot, op, tx, ty, tz):
     robot.features[op].feature.signal('selec').value = '000111'
     robot.tasks[op].signal('controlGain').value = 1.
 
+def sqrDist(value, expectedValue):
+    """Compute the square of the distance between two configurations."""
+    return reduce(lambda acc, (a, b): acc + abs(a - b) * abs(a - b),
+                  zip(value, expectedValue), 0.)
+
+def checkFinalConfiguration(position, finalPosition):
+    if sqrDist(position, finalPosition) >= 1e-3:
+        print "Wrong final position. Failing."
+        print "Value:"
+        displayHrp2Configuration(position)
+        print "Expected value:"
+        displayHrp2Configuration(finalPosition)
+        print "Difference:"
+        displayHrp2Configuration(map(lambda (x, y): x - y,
+                                     zip(position, finalPosition)))
+        sys.exit(1)
 
 ##################
 # Helper classes #