diff --git a/CMakeLists.txt b/CMakeLists.txt
index 83f1408f8372f35500b6217eaaf3dd267b65a2b5..4fb17c66efe4c08637d69ab38a5304346ecdbe57 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -44,8 +44,8 @@ ADD_REQUIRED_DEPENDENCY("jrl-dynamics >= 1.16.1")
ADD_REQUIRED_DEPENDENCY("hrp2-dynamics >= 1.3.0")
ADD_REQUIRED_DEPENDENCY("hrp2-10-optimized >= 1.0")
-ADD_REQUIRED_DEPENDENCY("hrp2_10 >= 1.0.0")
-ADD_REQUIRED_DEPENDENCY("hrp2_14 >= 1.0.0")
+ADD_REQUIRED_DEPENDENCY("hrp2-10 >= 1.0.0")
+ADD_REQUIRED_DEPENDENCY("hrp2-14 >= 1.0.0")
ADD_REQUIRED_DEPENDENCY("dynamic-graph >= 1.0.0")
ADD_REQUIRED_DEPENDENCY("sot-core >= 1.0.0")
diff --git a/include/sot-dynamic/angle-estimator.h b/include/sot-dynamic/angle-estimator.h
index 70153b372dfb6f3fabf4a8c322e3374a75df62b9..169d4389c238ff481415cbc69f806b0b7ea17754 100644
--- a/include/sot-dynamic/angle-estimator.h
+++ b/include/sot-dynamic/angle-estimator.h
@@ -66,6 +66,7 @@ class SOTANGLEESTIMATOR_EXPORT AngleEstimator
{
public:
static const std::string CLASS_NAME;
+ virtual const std::string& getClassName( void ) const { return CLASS_NAME; }
public: /* --- CONSTRUCTION --- */
diff --git a/include/sot-dynamic/dynamic-hrp2.h b/include/sot-dynamic/dynamic-hrp2.h
index 12f1a0d4118a3d4648f48df6471d506a1c5ed747..416016b900c4c00112c8664300c57e84a5ce9ee0 100644
--- a/include/sot-dynamic/dynamic-hrp2.h
+++ b/include/sot-dynamic/dynamic-hrp2.h
@@ -65,6 +65,10 @@ class SOTDYNAMICHRP2_EXPORT DynamicHrp2
{
public:
+ virtual const std::string& getClassName () const
+ {
+ return CLASS_NAME;
+ }
static const std::string CLASS_NAME;
public: /* --- CONSTRUCTION --- */
diff --git a/include/sot-dynamic/dynamic-hrp2_10.h b/include/sot-dynamic/dynamic-hrp2_10.h
index a3908f6980fb5ec98b274f71cf2068133f910cc0..2d97df514545ad7f0df03a6a0d3c83eecb41e315 100644
--- a/include/sot-dynamic/dynamic-hrp2_10.h
+++ b/include/sot-dynamic/dynamic-hrp2_10.h
@@ -67,6 +67,10 @@ class DYNAMICHRP2_10_EXPORT DynamicHrp2_10
DynamicHrp2_10( const std::string& name );
virtual ~DynamicHrp2_10( void );
+ virtual const std::string& getClassName () const
+ {
+ return CLASS_NAME;
+ }
public: /* --- MODEL CREATION --- */
diff --git a/include/sot-dynamic/dynamic.h b/include/sot-dynamic/dynamic.h
index 7aa63f10ad4d79791acf012697e88c0dfb6111f1..a21c5bbdbe16354541660da4a81dce5b1e84eef4 100644
--- a/include/sot-dynamic/dynamic.h
+++ b/include/sot-dynamic/dynamic.h
@@ -93,7 +93,7 @@ class SOTDYNAMIC_EXPORT Dynamic
friend class sot::command::InitializeRobot;
public:
- static const std::string CLASS_NAME;
+ DYNAMIC_GRAPH_ENTITY_DECL();
protected:
public:
diff --git a/include/sot-dynamic/force-compensation.h b/include/sot-dynamic/force-compensation.h
index bc7e72c8da14d2c1a42c964c02d2b4d9d767e93d..09fd8405e41721b0488c706f14633b0aaecc5545 100644
--- a/include/sot-dynamic/force-compensation.h
+++ b/include/sot-dynamic/force-compensation.h
@@ -136,6 +136,7 @@ namespace dynamicgraph { namespace sot {
{
public:
static const std::string CLASS_NAME;
+ virtual const std::string& getClassName( void ) const { return CLASS_NAME; }
bool calibrationStarted;
diff --git a/include/sot-dynamic/integrator-force.h b/include/sot-dynamic/integrator-force.h
index 5b9af5ea826a2e21c82efe5af2033aaefa2a8016..6abc8e2edde5b26569a2b138d7e0e81f904db24c 100644
--- a/include/sot-dynamic/integrator-force.h
+++ b/include/sot-dynamic/integrator-force.h
@@ -67,6 +67,7 @@ class SOTINTEGRATORFORCE_EXPORT IntegratorForce
{
public:
static const std::string CLASS_NAME;
+ virtual const std::string& getClassName( void ) const { return CLASS_NAME; }
protected:
double timeStep;
diff --git a/include/sot-dynamic/mass-apparent.h b/include/sot-dynamic/mass-apparent.h
index e54fa65e1274ae2556b4c6e17cdc5f012617b0c7..0d5d77617006dbf77a3fa15053a3066dddd219a7 100644
--- a/include/sot-dynamic/mass-apparent.h
+++ b/include/sot-dynamic/mass-apparent.h
@@ -66,6 +66,7 @@ namespace dynamicgraph { namespace sot {
{
public:
static const std::string CLASS_NAME;
+ virtual const std::string& getClassName( void ) const { return CLASS_NAME; }
public: /* --- CONSTRUCTION --- */
diff --git a/include/sot-dynamic/waist-attitude-from-sensor.h b/include/sot-dynamic/waist-attitude-from-sensor.h
index 2a74e30dd7071a40fa091a3f414581076cf77f09..ee96d97a125648d6ec6fd3f83b202f2129af7ede 100644
--- a/include/sot-dynamic/waist-attitude-from-sensor.h
+++ b/include/sot-dynamic/waist-attitude-from-sensor.h
@@ -66,6 +66,7 @@ class SOTWAISTATTITUDEFROMSENSOR_EXPORT WaistAttitudeFromSensor
{
public:
static const std::string CLASS_NAME;
+ virtual const std::string& getClassName( void ) const { return CLASS_NAME; }
public: /* --- CONSTRUCTION --- */
diff --git a/include/sot-dynamic/zmpreffromcom.h b/include/sot-dynamic/zmpreffromcom.h
index 2f75df546b4c2a9a343bdb1f27a266206a8d88c2..58e78e4aec7d0154f4504dd18787b6a1a491cbaa 100644
--- a/include/sot-dynamic/zmpreffromcom.h
+++ b/include/sot-dynamic/zmpreffromcom.h
@@ -64,6 +64,8 @@ class SOTZMPREFFROMCOM_EXPORT ZmprefFromCom
{
public:
static const std::string CLASS_NAME;
+ virtual const std::string& getClassName( void ) const { return CLASS_NAME; }
+ public:
double dt;
const static double DT_DEFAULT; // = 5e-3; // 5ms
double footHeight;
diff --git a/src/dynamic_graph/sot/dynamics/hrp2.py.in b/src/dynamic_graph/sot/dynamics/hrp2.py.in
index 1a71185624e585728632dda618947c4af1f7662d..93e8ed2c5200fbd062060c343ddb4c597b90efe0 100755
--- a/src/dynamic_graph/sot/dynamics/hrp2.py.in
+++ b/src/dynamic_graph/sot/dynamics/hrp2.py.in
@@ -22,6 +22,7 @@ from dynamic_graph import enableTrace, plug
from dynamic_graph.sot.se3 import R3, SO3, SE3
from dynamic_graph.sot.dynamics.dynamic_hrp2 import DynamicHrp2
+from dynamic_graph.sot.dynamics.dynamic_hrp2_10 import DynamicHrp2_10
from dynamic_graph.sot.dynamics.humanoid_robot import AbstractHumanoidRobot
@@ -38,7 +39,7 @@ class Hrp2(AbstractHumanoidRobot):
res = (config + 10*(0.,))
return res
- def __init__(self, name, modelDir, xmlDir, device):
+ def __init__(self, name, modelDir, xmlDir, device, dynamicType):
AbstractHumanoidRobot.__init__ (self, name)
self.OperationalPoints.append('waist')
@@ -47,10 +48,14 @@ class Hrp2(AbstractHumanoidRobot):
specificitiesPath = xmlDir + '/HRP2SpecificitiesSmall.xml'
jointRankPath = xmlDir + '/HRP2LinkJointRankSmall.xml'
- self.dynamic = DynamicHrp2(self.name + '_dynamic')
- self.dynamic.setFiles(modelDir, modelName,
- specificitiesPath, jointRankPath)
- self.dynamic.parse()
+ self.dynamic = self.loadModelFromJrlDynamics(
+ self.name + '_dynamic',
+ modelDir,
+ modelName,
+ specificitiesPath,
+ jointRankPath,
+ dynamicType)
+
self.dimension = self.dynamic.getDimension()
if self.dimension != len(self.halfSitting):
raise RuntimeError("invalid half-sitting pose")
@@ -80,7 +85,7 @@ class Hrp2Jrl (Hrp2):
modelDir = hrp2_10_pkgdatarootdir,
xmlDir = hrp2_10_pkgdatarootdir,
device = None):
- Hrp2.__init__(self, name, modelDir, xmlDir, device)
+ Hrp2.__init__(self, name, modelDir, xmlDir, device, DynamicHrp2_10)
class Hrp2Laas (Hrp2):
"""
@@ -106,4 +111,4 @@ class Hrp2Laas (Hrp2):
modelDir = hrp2_14_pkgdatarootdir,
xmlDir = hrp2_14_pkgdatarootdir,
device = None):
- Hrp2.__init__(self, name, modelDir, xmlDir, device)
+ Hrp2.__init__(self, name, modelDir, xmlDir, device, DynamicHrp2)
diff --git a/src/dynamic_graph/sot/dynamics/humanoid_robot.py b/src/dynamic_graph/sot/dynamics/humanoid_robot.py
index 03c376d26c81d960678809a9a202fb6e7ca926b3..05634fe8d9b0deab749ec6759307f5e7a030e2a4 100755
--- a/src/dynamic_graph/sot/dynamics/humanoid_robot.py
+++ b/src/dynamic_graph/sot/dynamics/humanoid_robot.py
@@ -124,7 +124,8 @@ class AbstractHumanoidRobot (object):
return model
def loadModelFromJrlDynamics(self, name, modelDir, modelName,
- specificitiesPath, jointRankPath):
+ specificitiesPath, jointRankPath,
+ dynamicType):
"""
Load a model using the jrl-dynamics parser. This parser looks
for VRML files in which kinematics and dynamics information
@@ -134,12 +135,11 @@ class AbstractHumanoidRobot (object):
Additional information are located in two different XML files.
"""
- #FIXME: add support for hrp2-10 here.
- model = DynamicHrp2(name)
+ model = dynamicType(name)
model.setFiles(modelDir, modelName,
specificitiesPath, jointRankPath)
model.parse()
- return
+ return model
def initializeOpPoints(self, model):
for op in self.OperationalPoints:
@@ -212,6 +212,28 @@ class AbstractHumanoidRobot (object):
def __init__(self, name):
self.name = name
+ def reset(self, posture = None):
+ """
+ Restart the control from another position.
+
+ This method has not been extensively tested and
+ should be used carefully.
+
+ In particular, tasks should be removed from the
+ solver before attempting a reset.
+ """
+ if not posture:
+ posture = self.halfSitting
+ self.device.set(posture)
+
+ self.dynamic.com.recompute(self.device.state.time+1)
+ self.dynamic.Jcom.recompute(self.device.state.time+1)
+ self.featureComDes.errorIN.value = self.dynamic.com.value
+
+ for op in self.OperationalPoints:
+ self.dynamic.signal(op).recompute(self.device.state.time+1)
+ self.dynamic.signal('J'+op).recompute(self.device.state.time+1)
+ self.features[op].reference.value = self.dynamic.signal(op).value
class HumanoidRobot(AbstractHumanoidRobot):
diff --git a/src/dynamic_graph/sot/dynamics/solver.py b/src/dynamic_graph/sot/dynamics/solver.py
index 51b27b183d464277cf2884b8dabc742cb4c0abd7..1be5988b798546111fbaa6c5d8da98ad521cb95b 100644
--- a/src/dynamic_graph/sot/dynamics/solver.py
+++ b/src/dynamic_graph/sot/dynamics/solver.py
@@ -16,7 +16,7 @@
# dynamic-graph. If not, see <http://www.gnu.org/licenses/>.
from dynamic_graph import plug
-from dynamic_graph.sot.core import SOT
+from dynamic_graph.sot.core import JointLimitator, SOT
class Solver:
robot = None
@@ -24,14 +24,28 @@ class Solver:
def __init__(self, robot):
self.robot = robot
+
+ # Make sure control does not exceed joint limits.
+ self.jointLimitator = JointLimitator('joint_limitator')
+ plug(self.robot.dynamic.position, self.jointLimitator.joint)
+ plug(self.robot.dynamic.upperJl, self.jointLimitator.upperJl)
+ plug(self.robot.dynamic.lowerJl, self.jointLimitator.lowerJl)
+
+ # Create the solver.
self.sot = SOT('solver')
self.sot.signal('damping').value = 1e-6
self.sot.setNumberDofs(self.robot.dimension)
+ # Plug the solver control into the filter.
+ plug(self.sot.control, self.jointLimitator.controlIN)
+
+ # Important: always use 'jointLimitator.control'
+ # and NOT 'sot.control'!
+
if robot.device:
- plug(self.sot.signal('control'), robot.device.signal('control'))
- plug(self.robot.device.state,
- self.robot.dynamic.position)
+ plug(self.jointLimitator.control, robot.device.control)
+ plug(self.robot.device.state, self.robot.dynamic.position)
+
def push(self, taskName):
"""
Proxy method to push a task in the sot
diff --git a/src/dynamic_graph/sot/dynamics/tools.py b/src/dynamic_graph/sot/dynamics/tools.py
index 5b1f80c9aa131296d762357962d2e7ef83f29960..b4e670bec200bf23236b9686d11da43449ae8d99 100755
--- a/src/dynamic_graph/sot/dynamics/tools.py
+++ b/src/dynamic_graph/sot/dynamics/tools.py
@@ -151,6 +151,9 @@ if 'argv' in sys.__dict__:
parser.add_option("-d", "--display",
action="store_true", dest="display", default=False,
help="enable display using robotviewer")
+ parser.add_option("-r", "--robot",
+ action="store", dest="robot", default="Hrp2Laas",
+ help="Specify which robot model to use")
(options, args) = parser.parse_args()
if options.display:
@@ -162,6 +165,13 @@ if 'argv' in sys.__dict__:
# Initialize the stack of tasks.
- robot = Hrp2Laas("robot")
+ robots = {
+ "Hrp2Laas" : Hrp2Laas,
+ "Hrp2Jrl" : Hrp2Jrl
+ }
+ if not options.robot in robots:
+ raise RuntimeError (
+ "invalid robot name (should by Hrp2Laas or Hrp2Jrl)")
+ robot = robots[options.robot]("robot")
timeStep = .005
solver = Solver(robot)
diff --git a/unitTesting/python/dynamic_walk.py b/unitTesting/python/dynamic_walk.py
index 7b36e463a5d210b8efd83cfe1a944da75176d477..23699b17e16be0923a936717643729ae6faa97b3 100755
--- a/unitTesting/python/dynamic_walk.py
+++ b/unitTesting/python/dynamic_walk.py
@@ -24,8 +24,7 @@ from dynamic_graph.sot.dynamics.hrp2 import Hrp2
from dynamic_graph import enableTrace, plug
-from tools import *
-
+from dynamic_graph.sot.dynamics.tools import *
class HalfStep:
startX = 0.
diff --git a/unitTesting/python/feet_follower.py b/unitTesting/python/feet_follower.py
index 043179d7160ddf257bd08f8fd01af8f10071eb0b..1576e439103ae95954ee4d9f29536ea8b2a3e279 100755
--- a/unitTesting/python/feet_follower.py
+++ b/unitTesting/python/feet_follower.py
@@ -15,7 +15,7 @@
# 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.tools import *
from dynamic_graph.sot.dynamics.feet_follower import FeetFollowerFromFile
feetFollower = FeetFollowerFromFile('feet-follower')
diff --git a/unitTesting/python/quasistatic_walking.py b/unitTesting/python/quasistatic_walking.py
index e3608d4576cf60093e5301eb49cfa672b1321b4b..3de2bc0e4f003cd8626be8cf1a26d63aec18780d 100755
--- a/unitTesting/python/quasistatic_walking.py
+++ b/unitTesting/python/quasistatic_walking.py
@@ -21,8 +21,7 @@ from dynamic_graph.sot.dynamics.hrp2 import Hrp2
from dynamic_graph import enableTrace, plug
-from tools import *
-
+from dynamic_graph.sot.dynamics.tools import *
class QuasiStaticWalking:
leftFoot = 0
@@ -155,13 +154,12 @@ class QuasiStaticWalking:
# Push tasks
# Feet tasks.
-solver.sot.push(robot.name + '.task.right-ankle')
-solver.sot.push(robot.name + '.task.left-ankle')
+solver.sot.push(robot.tasks['right-ankle'].name)
+solver.sot.push(robot.tasks['left-ankle'].name)
# Center of mass
# FIXME: trigger segv at exit.
-solver.sot.push(robot.name + '.task.com')
-
+solver.sot.push(robot.comTask.name)
# Main.
diff --git a/unitTesting/python/reach_both_hands.py b/unitTesting/python/reach_both_hands.py
index 9658c8d6473096541d3559e8df957c6cd892d6c4..ab853dc52f0f4aed7927affe4c16e08e437c4b0a 100755
--- a/unitTesting/python/reach_both_hands.py
+++ b/unitTesting/python/reach_both_hands.py
@@ -16,7 +16,7 @@
# dynamic-graph. If not, see <http://www.gnu.org/licenses/>.
import sys
-from tools import *
+from dynamic_graph.sot.dynamics.tools import *
# Move left wrist
reach(robot, 'left-wrist', 0.25, 0.25, 0.5)
@@ -24,13 +24,13 @@ 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')
+solver.sot.push(robot.tasks['right-ankle'].name)
+solver.sot.push(robot.tasks['left-ankle'].name)
+solver.sot.push(robot.tasks['right-wrist'].name)
+solver.sot.push(robot.tasks['left-wrist'].name)
# Center of mass
-solver.sot.push(robot.name + '.task.com')
+solver.sot.push(robot.comTask.name)
# Main.
# Main loop
diff --git a/unitTesting/python/reach_left_hand.py b/unitTesting/python/reach_left_hand.py
index 941024745059cf6901a9e0f46d749cf38fec355c..bbb8e0e35613e2347cd98a39f7227a7ffd0bf099 100755
--- a/unitTesting/python/reach_left_hand.py
+++ b/unitTesting/python/reach_left_hand.py
@@ -16,20 +16,20 @@
# dynamic-graph. If not, see <http://www.gnu.org/licenses/>.
import sys
-from tools import *
+from dynamic_graph.sot.dynamics.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')
+solver.sot.push(robot.tasks['right-ankle'].name)
+solver.sot.push(robot.tasks['left-ankle'].name)
+solver.sot.push(robot.tasks['right-wrist'].name)
+solver.sot.push(robot.tasks['left-wrist'].name)
# Center of mass
-solver.sot.push(robot.name + '.task.com')
+solver.sot.push(robot.comTask.name)
# Main.
# Main loop
diff --git a/unitTesting/python/reach_right_hand.py b/unitTesting/python/reach_right_hand.py
index 834cef6d4f2d72f0e224b093a10a0588b2ec5c73..e6fb07f7ec9ae07514be505b1543e7e39e72556c 100755
--- a/unitTesting/python/reach_right_hand.py
+++ b/unitTesting/python/reach_right_hand.py
@@ -15,20 +15,20 @@
# 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.tools import *
# Move right wrist
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')
+solver.sot.push(robot.tasks['right-ankle'].name)
+solver.sot.push(robot.tasks['left-ankle'].name)
+solver.sot.push(robot.tasks['right-wrist'].name)
+solver.sot.push(robot.tasks['left-wrist'].name)
# Center of mass
-solver.sot.push(robot.name + '.task.com')
+solver.sot.push(robot.comTask.name)
# Main.
# Main loop