From bcf47aff87cbd6c4c0b468e000466e53336f53c9 Mon Sep 17 00:00:00 2001
From: Thomas Moulard <thomas.moulard@gmail.com>
Date: Wed, 28 Sep 2011 11:47:54 +0200
Subject: [PATCH] Add support for additional frames storing sensors position,
use it to compute HRP-2 camera positions.
---
src/dynamic_graph/sot/dynamics/hrp2.py.in | 61 +++++++++++++++++++
.../sot/dynamics/humanoid_robot.py | 37 ++++++++++-
2 files changed, 97 insertions(+), 1 deletion(-)
diff --git a/src/dynamic_graph/sot/dynamics/hrp2.py.in b/src/dynamic_graph/sot/dynamics/hrp2.py.in
index 93e8ed2..f1ee904 100755
--- a/src/dynamic_graph/sot/dynamics/hrp2.py.in
+++ b/src/dynamic_graph/sot/dynamics/hrp2.py.in
@@ -14,6 +14,7 @@
# received a copy of the GNU Lesser General Public License along with
# dynamic-graph. If not, see <http://www.gnu.org/licenses/>.
+import numpy as np
from dynamic_graph.sot import SE3, R3, SO3
from dynamic_graph.sot.core import FeaturePoint6dRelative, \
FeatureGeneric, FeatureJointLimits, Task, Constraint, GainAdaptive, SOT
@@ -30,6 +31,14 @@ from dynamic_graph.sot.dynamics.humanoid_robot import AbstractHumanoidRobot
hrp2_10_pkgdatarootdir = "@HRP2_10_PKGDATAROOTDIR@"
hrp2_14_pkgdatarootdir = "@HRP2_14_PKGDATAROOTDIR@"
+# Internal helper tool.
+def matrixToTuple(M):
+ tmp = M.tolist()
+ res = []
+ for i in tmp:
+ res.append(tuple(i))
+ return tuple(res)
+
class Hrp2(AbstractHumanoidRobot):
"""
@@ -111,4 +120,56 @@ class Hrp2Laas (Hrp2):
modelDir = hrp2_14_pkgdatarootdir,
xmlDir = hrp2_14_pkgdatarootdir,
device = None):
+
+ # Define camera positions w.r.t gaze.
+ cameraBottomLeftPosition = np.matrix((
+ (1., 0., 0., 0.035),
+ (0., 1., 0., 0.072),
+ (0., 0., 1., 0.075),
+ (0., 0., 0., 1.),
+ ))
+ cameraBottomRightPosition = np.matrix((
+ (1., 0., 0., 0.035),
+ (0., 1., 0., -0.072),
+ (0., 0., 1., 0.075),
+ (0., 0., 0., 1.),
+ ))
+ cameraTopLeftPosition = np.matrix((
+ (0.99920, 0.00120, 0.03997, 0.01),
+ (0.00000, 0.99955,-0.03000, 0.029),
+ (-0.03999, 0.02997, 0.99875, 0.145),
+ (0., 0., 0., 1.),
+ ))
+ cameraTopRightPosition = np.matrix((
+ (0.99920, 0.00000, 0.03999, 0.01),
+ (0.00000, 1.00000, 0.00000, -0.029),
+ (-0.03999, 0.00000, 0.99920, 0.145),
+ (0., 0., 0., 1.),
+ ))
+
+ # Frames re-orientation:
+ # Z = depth (increase from near to far)
+ # X = increase from left to right
+ # Y = increase from top to bottom
+ c1_M_c = np.matrix(
+ [[ 0., 0., 1., 0.],
+ [-1., 0., 0., 0.],
+ [ 0., -1., 0., 0.],
+ [ 0., 0., 0., 1.]])
+
+ for camera in [cameraBottomLeftPosition, cameraBottomRightPosition,
+ cameraTopLeftPosition, cameraTopRightPosition]:
+ camera = camera * c1_M_c
+
+ self.AdditionalFrames.append(
+ ("cameraBottomLeft", matrixToTuple(cameraBottomLeftPosition), "gaze"))
+ self.AdditionalFrames.append(
+ ("cameraBottomRight", matrixToTuple(cameraBottomRightPosition), "gaze"))
+ self.AdditionalFrames.append(
+ ("cameraTopLeft", matrixToTuple(cameraTopLeftPosition), "gaze"))
+ self.AdditionalFrames.append(
+ ("cameraTopRight", matrixToTuple(cameraTopRightPosition), "gaze"))
+
Hrp2.__init__(self, name, modelDir, xmlDir, device, DynamicHrp2)
+
+__all__ = [Hrp2, Hrp2Jrl, Hrp2Laas]
diff --git a/src/dynamic_graph/sot/dynamics/humanoid_robot.py b/src/dynamic_graph/sot/dynamics/humanoid_robot.py
index 2aa8054..44090e4 100755
--- a/src/dynamic_graph/sot/dynamics/humanoid_robot.py
+++ b/src/dynamic_graph/sot/dynamics/humanoid_robot.py
@@ -15,6 +15,7 @@
# dynamic-graph. If not, see <http://www.gnu.org/licenses/>.
from dynamic_graph.sot import SE3, R3, SO3
+from dynamic_graph.sot.core import OpPointModifier
from dynamic_graph.sot.core.derivator import Derivator_of_Vector
from dynamic_graph.sot.core.feature_position import FeaturePosition
from dynamic_graph.sot.core import RobotSimu, FeaturePoint6dRelative, \
@@ -57,6 +58,19 @@ class AbstractHumanoidRobot (object):
"Jleft-wrist" for respectively the position and the jacobian.
"""
+ AdditionalFrames = []
+ """
+ Additional frames are frames which are defined w.r.t an operational point
+ and provides an interesting transformation.
+
+ It can be used, for instance, to store the sensor location.
+
+ The contained elements must be triplets matching:
+ - additional frame name,
+ - transformation w.r.t to the operational point,
+ - operational point file.
+ """
+
name = None
"""Entity name (internal use)"""
@@ -102,6 +116,11 @@ class AbstractHumanoidRobot (object):
corresponding to operational points.
"""
+ frames = dict()
+ """
+ Additional frames defined by using OpPointModifier.
+ """
+
#FIXME: the following options are /not/ independent.
# zmp requires acceleration which requires velocity.
"""
@@ -147,10 +166,10 @@ class AbstractHumanoidRobot (object):
Additional information are located in two different XML files.
"""
model = dynamicType(name)
+ self.setProperties(model)
model.setFiles(modelDir, modelName,
specificitiesPath, jointRankPath)
model.parse()
- self.setProperties(model)
return model
def setProperties(self, model):
@@ -259,6 +278,22 @@ class AbstractHumanoidRobot (object):
memberName += i.capitalize()
setattr(self, memberName, self.features[op])
+ # --- additional frames ---
+ self.frame = dict()
+ for (frameName, transformation, signalName) in self.AdditionalFrames:
+ self.frame[frameName] = OpPointModifier(
+ "{0}_{1}".format(self.name, frameName))
+ self.frame[frameName].setTransformation(transformation)
+ plug(self.dynamic.signal(signalName),
+ self.frame[frameName].positionIN)
+ plug(self.dynamic.signal("J{0}".format(signalName)),
+ self.frame[frameName].jacobianIN)
+
+ self.frame[frameName].position.recompute(
+ self.frame[frameName].position.time + 1)
+ self.frame[frameName].jacobian.recompute(
+ self.frame[frameName].jacobian.time + 1)
+
def __init__(self, name):
self.name = name
--
GitLab