From 1c5d987589da3df2953db3e3f431eec3cd9755fd Mon Sep 17 00:00:00 2001
From: Thomas Moulard <thomas.moulard@gmail.com>
Date: Tue, 11 Jan 2011 14:57:51 +0100
Subject: [PATCH] Add new tests.

---
 unitTesting/python/reach_both_hands.py | 43 ++++++++++++++++++++++++++
 unitTesting/python/reach_left_hand.py  | 42 +++++++++++++++++++++++++
 unitTesting/python/reach_right_hand.py | 16 ++--------
 unitTesting/python/tools.py            | 10 ++++++
 4 files changed, 97 insertions(+), 14 deletions(-)
 create mode 100755 unitTesting/python/reach_both_hands.py
 create mode 100755 unitTesting/python/reach_left_hand.py

diff --git a/unitTesting/python/reach_both_hands.py b/unitTesting/python/reach_both_hands.py
new file mode 100755
index 0000000..a7c8837
--- /dev/null
+++ b/unitTesting/python/reach_both_hands.py
@@ -0,0 +1,43 @@
+#!/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 *
+
+# Move left wrist
+reach(robot, 'left-wrist', 0.25, 0.25, 0.5)
+reach(robot, 'right-wrist', 0.25, -0.25, 0.5)
+
+# Push tasks
+#  Operational points tasks
+solver.sot.push(robot.name + '.task.right-ankle')
+solver.sot.push(robot.name + '.task.left-ankle')
+solver.sot.push(robot.name + '.task.left-wrist')
+solver.sot.push(robot.name + '.task.right-wrist')
+
+#  Center of mass
+solver.sot.push(robot.name + '.task.com')
+
+# Main.
+#  Main loop
+for i in xrange(500):
+    robot.robotSimu.increment(timeStep)
+
+    if clt:
+        clt.updateElementConfig(
+            'hrp', robot.smallToFull(robot.robotSimu.signal('state').value))
+
+print "FINISHED"
diff --git a/unitTesting/python/reach_left_hand.py b/unitTesting/python/reach_left_hand.py
new file mode 100755
index 0000000..89798f8
--- /dev/null
+++ b/unitTesting/python/reach_left_hand.py
@@ -0,0 +1,42 @@
+#!/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 *
+
+# Move left wrist
+reach(robot, 'left-wrist', 0.25, 0, 0.1)
+
+# Push tasks
+#  Operational points tasks
+solver.sot.push(robot.name + '.task.right-ankle')
+solver.sot.push(robot.name + '.task.left-ankle')
+solver.sot.push(robot.name + '.task.left-wrist')
+solver.sot.push(robot.name + '.task.right-wrist')
+
+#  Center of mass
+solver.sot.push(robot.name + '.task.com')
+
+# Main.
+#  Main loop
+for i in xrange(500):
+    robot.robotSimu.increment(timeStep)
+
+    if clt:
+        clt.updateElementConfig(
+            'hrp', robot.smallToFull(robot.robotSimu.signal('state').value))
+
+print "FINISHED"
diff --git a/unitTesting/python/reach_right_hand.py b/unitTesting/python/reach_right_hand.py
index ce18882..a42f5d6 100755
--- a/unitTesting/python/reach_right_hand.py
+++ b/unitTesting/python/reach_right_hand.py
@@ -15,28 +15,16 @@
 # received a copy of the GNU Lesser General Public License along with
 # dynamic-graph. If not, see <http://www.gnu.org/licenses/>.
 
-from dynamic_graph.sot.core import FeaturePoint6d, FeatureGeneric, SOT
-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 *
 
-
 # Move right wrist
-sdes = toList(robot.dynamicRobot.signal('right-wrist').value)
-sdes[0][3] += 0.25 # Move reference point forward.
-sdes[2][3] += 0.1 # Increment reference point altitude.
-robot.features['right-wrist'].reference.signal('position').value = toTuple(sdes)
-# Select translation only.
-robot.features['right-wrist'].feature.signal('selec').value = '000111'
-robot.tasks['right-wrist'].signal('controlGain').value = 1.
+reach(robot, 'right-wrist', 0.25, 0, 0.1)
 
 # Push tasks
 #  Operational points tasks
 solver.sot.push(robot.name + '.task.right-ankle')
 solver.sot.push(robot.name + '.task.left-ankle')
+solver.sot.push(robot.name + '.task.left-wrist')
 solver.sot.push(robot.name + '.task.right-wrist')
 
 #  Center of mass
diff --git a/unitTesting/python/tools.py b/unitTesting/python/tools.py
index 317d139..6dbb5c0 100755
--- a/unitTesting/python/tools.py
+++ b/unitTesting/python/tools.py
@@ -82,6 +82,16 @@ def initRobotViewer():
             print "failed to connect to robotviewer"
     return clt
 
+def reach(robot, op, tx, ty, tz):
+    sdes = toList(robot.dynamicRobot.signal(op).value)
+    sdes[0][3] += tx
+    sdes[1][3] += ty
+    sdes[2][3] += tz
+    robot.features[op].reference.signal('position').value = toTuple(sdes)
+    # Select translation only.
+    robot.features[op].feature.signal('selec').value = '000111'
+    robot.tasks[op].signal('controlGain').value = 1.
+
 
 ##################
 # Helper classes #
-- 
GitLab