diff --git a/src/dynamic_graph/sot/dynamics/hrp2.py.in b/src/dynamic_graph/sot/dynamics/hrp2.py.in
index 5cfd0829241610a7548adafa4a2759e15843173b..63b1c314571a68e25d49172acbe97ba0cb79b2fb 100755
--- a/src/dynamic_graph/sot/dynamics/hrp2.py.in
+++ b/src/dynamic_graph/sot/dynamics/hrp2.py.in
@@ -57,6 +57,9 @@ class Hrp2(AbstractHumanoidRobot):
0.261799, 0.17453, 0., -0.523599, 0., 0., 0.1,
)
+ def smallToFull(self, config):
+ res = (config + 10*(0.,))
+ return res
def __init__(self, name,
simu,
diff --git a/src/dynamic_graph/sot/dynamics/humanoid_robot.py b/src/dynamic_graph/sot/dynamics/humanoid_robot.py
index 7696b1cdbe4cd52a57658d65676b57ccb329e463..eb03da556a4b7b4b3ea83a1da3666a56877ced98 100755
--- a/src/dynamic_graph/sot/dynamics/humanoid_robot.py
+++ b/src/dynamic_graph/sot/dynamics/humanoid_robot.py
@@ -204,6 +204,17 @@ class AbstractHumanoidRobot (object):
else:
self.simu = False
+ def restart(self):
+ if self.robotSimu:
+ self.robotSimu.set(self.halfSitting)
+ self.dynamicRobot.signal('position').value = self.halfSitting
+ self.dynamicRobot.signal('velocity').value = self.dimension * (0.,)
+ self.dynamicRobot.signal('acceleration').value = self.dimension * (0.,)
+ if self.robotSimu:
+ self.robotSimu.increment(.001)
+
+
+
class HumanoidRobot(AbstractHumanoidRobot):
diff --git a/unitTesting/python/quasistatic_walking.py b/unitTesting/python/quasistatic_walking.py
index dd83b1f487f3c3c57ac4bac9aa7394df296be3c3..cf146ad649134fd15bbad2a8f35ef338c543b617 100755
--- a/unitTesting/python/quasistatic_walking.py
+++ b/unitTesting/python/quasistatic_walking.py
@@ -21,57 +21,9 @@ from dynamic_graph.sot.dynamics.hrp2 import Hrp2
from dynamic_graph import enableTrace, plug
-# Robotviewer is optional
-enableRobotViewer = True
-try:
- import robotviewer
-except ImportError:
- enableRobotViewer = False
+from tools import *
-
-def displayHomogeneousMatrix(matrix):
- """
- Display nicely a 4x4 matrix (usually homogeneous matrix).
- """
- import itertools
-
- matrix_tuple = tuple(itertools.chain.from_iterable(matrix))
-
- formatStr = ''
- for i in xrange(4*4):
- formatStr += '{0[' + str(i) + ']: <10} '
- if i != 0 and (i + 1) % 4 == 0:
- formatStr += '\n'
- print formatStr.format(matrix_tuple)
-
-def toList(tupleOfTuple):
- result = [[0, 0, 0, 0],
- [0, 0, 0, 0],
- [0, 0, 0, 0],
- [0, 0, 0, 0]]
- for i in xrange(4):
- for j in xrange(4):
- result[i][j] = tupleOfTuple[i][j]
- return result
-
-def toTuple(listOfList):
- return ((listOfList[0][0], listOfList[0][1],
- listOfList[0][2], listOfList[0][3]),
-
- (listOfList[1][0], listOfList[1][1],
- listOfList[1][2], listOfList[1][3]),
-
- (listOfList[2][0], listOfList[2][1],
- listOfList[2][2], listOfList[2][3]),
-
- (listOfList[3][0], listOfList[3][1],
- listOfList[3][2], listOfList[3][3]))
-
-def smallToFull(config):
- res = (config + 10*(0.,))
- return res
-
class QuasiStaticWalking:
leftFoot = 0
rightFoot = 1
@@ -200,34 +152,6 @@ class QuasiStaticWalking:
# Trigger actions to move to next state.
self.do(self.state)
-robot = Hrp2("robot", True)
-
-# Initialize robotviewer is possible.
-clt = None
-if enableRobotViewer:
- try:
- clt = robotviewer.client()
- except:
- enableRobotViewer = False
-
-timeStep = .02
-
-class Solver:
- robot = None
- sot = None
-
- def __init__(self, robot):
- self.robot = robot
- self.sot = SOT('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'))
-
-solver = Solver (robot)
# Push tasks
# Feet tasks.
@@ -262,7 +186,7 @@ for i in xrange(totalSteps + 1):
if clt:
clt.updateElementConfig(
- 'hrp', smallToFull(robot.robotSimu.signal('state').value))
+ 'hrp', robot.smallToFull(robot.robotSimu.signal('state').value))
# Security: switch back to double support.
quasiStaticWalking.moveCoM('origin')
@@ -274,7 +198,7 @@ for i in xrange(int(duration / timeStep)):
if clt:
clt.updateElementConfig(
- 'hrp', smallToFull(robot.robotSimu.signal('state').value))
+ '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
new file mode 100755
index 0000000000000000000000000000000000000000..ce18882c43a64b72a74ebb6d2ed1532a6ddc5ca3
--- /dev/null
+++ b/unitTesting/python/reach_right_hand.py
@@ -0,0 +1,54 @@
+#!/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 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.
+
+# 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.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/tools.py b/unitTesting/python/tools.py
new file mode 100755
index 0000000000000000000000000000000000000000..317d139b8dc75bda40579d69922b2e0c39b52699
--- /dev/null
+++ b/unitTesting/python/tools.py
@@ -0,0 +1,129 @@
+#!/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 optparse import OptionParser
+
+from dynamic_graph import plug
+from dynamic_graph.sot.core import SOT
+from dynamic_graph.sot.dynamics.hrp2 import Hrp2
+
+
+# Robotviewer is optional
+hasRobotViewer = True
+try:
+ import robotviewer
+except ImportError:
+ hasRobotViewer = False
+
+
+####################
+# Helper functions #
+####################
+
+def toList(tupleOfTuple):
+ result = [[0, 0, 0, 0],
+ [0, 0, 0, 0],
+ [0, 0, 0, 0],
+ [0, 0, 0, 0]]
+ for i in xrange(4):
+ for j in xrange(4):
+ result[i][j] = tupleOfTuple[i][j]
+ return result
+
+def toTuple(listOfList):
+ return ((listOfList[0][0], listOfList[0][1],
+ listOfList[0][2], listOfList[0][3]),
+
+ (listOfList[1][0], listOfList[1][1],
+ listOfList[1][2], listOfList[1][3]),
+
+ (listOfList[2][0], listOfList[2][1],
+ listOfList[2][2], listOfList[2][3]),
+
+ (listOfList[3][0], listOfList[3][1],
+ listOfList[3][2], listOfList[3][3]))
+
+def displayHomogeneousMatrix(matrix):
+ """
+ Display nicely a 4x4 matrix (usually homogeneous matrix).
+ """
+ import itertools
+
+ matrix_tuple = tuple(itertools.chain.from_iterable(matrix))
+
+ formatStr = ''
+ for i in xrange(4*4):
+ formatStr += '{0[' + str(i) + ']: <10} '
+ if i != 0 and (i + 1) % 4 == 0:
+ formatStr += '\n'
+ print formatStr.format(matrix_tuple)
+
+def initRobotViewer():
+ """Initialize robotviewer is possible."""
+ clt = None
+ if hasRobotViewer:
+ try:
+ clt = robotviewer.client()
+ except:
+ print "failed to connect to robotviewer"
+ return clt
+
+
+##################
+# Helper classes #
+##################
+
+class Solver:
+ robot = None
+ sot = None
+
+ def __init__(self, robot):
+ self.robot = robot
+ self.sot = SOT('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'))
+
+
+##################
+# Initialization #
+##################
+
+# Parse options and enable robotviewer client if wanted.
+clt = None
+parser = OptionParser()
+parser.add_option("-d", "--display",
+ action="store_true", dest="display", default=False,
+ help="enable display using robotviewer")
+(options, args) = parser.parse_args()
+
+if options.display:
+ if not hasRobotViewer:
+ print "Failed to import robotviewer client library."
+ clt = initRobotViewer()
+ if not clt:
+ print "Failed to initialize robotviewer client library."
+
+
+# Initialize the stack of tasks.
+robot = Hrp2("robot", True)
+timeStep = .02
+solver = Solver(robot)