Merge remote-tracking branch 'main/devel' into devel

src/dynamic_graph/sot/pyrene/prologue.py

 # -*- coding: utf-8 -*-
 # Copyright 2011, Florent Lamiraux, Thomas Moulard, JRL, CNRS/AIST
 
+# sys.argv is not defined when running the remove interpreter, but it is
+# required by rospy
+import sys
+
 from dynamic_graph.entity import PyEntityFactoryClass
 from dynamic_graph.sot.pyrene.robot import Robot
 
+if not hasattr(sys, 'argv'):
+    sys.argv = [
+        "dynamic_graph",
+    ]
+
 print("Prologue Pyrene TALOS Robot")
 
 
@@ -19,8 +28,7 @@ def makeRobot():
     DeviceTalos = PyEntityFactoryClass('DeviceTalos')
 
     # Create the robot using the device.
-    robot = Robot(name='robot', device=DeviceTalos('PYRENE'))
-
+    robot = Robot(name='robot', device=DeviceTalos('PYRENE'), fromRosParam=True)
     robot.dynamic.com.recompute(0)
     _com = robot.dynamic.com.value
     robot.device.zmp.value = (_com[0], _com[1], 0.)

src/dynamic_graph/sot/pyrene/robot.py

@@ -50,7 +50,7 @@ class Robot(Talos):
         0.  # Head
     )
 
-    def __init__(self, name, device=None, tracer=None):
+    def __init__(self, name, device=None, tracer=None, fromRosParam=None):
         Talos.__init__(self, name, self.halfSitting, device, tracer)
         """
         TODO:Confirm these values

src/dynamic_graph/sot/talos/talos.py

@@ -3,12 +3,13 @@
 
 from __future__ import print_function
 
-import pinocchio as se3
 from dynamic_graph import plug
 from dynamic_graph.sot.core.math_small_entities import Derivator_of_Vector
 from dynamic_graph.sot.dynamics_pinocchio import DynamicPinocchio
 from dynamic_graph.sot.dynamics_pinocchio.humanoid_robot import AbstractHumanoidRobot
+from pinocchio import JointModelFreeFlyer, buildModelFromXML, buildReducedModel, neutral
 from pinocchio.robot_wrapper import RobotWrapper
+from rospkg import RosPack
 
 
 # Internal helper tool.
@@ -27,6 +28,7 @@ class Talos(AbstractHumanoidRobot):
 
     forceSensorInLeftAnkle = ((1., 0., 0., 0.), (0., 1., 0., 0.), (0., 0., 1., -0.107), (0., 0., 0., 1.))
     forceSensorInRightAnkle = ((1., 0., 0., 0.), (0., 1., 0., 0.), (0., 0., 1., -0.107), (0., 0., 0., 1.))
+    defaultFilename = "talos_reduced_v2.urdf"
     """
     TODO: Confirm the position and existence of these sensors
     accelerometerPosition = np.matrix ((
@@ -50,7 +52,7 @@ class Talos(AbstractHumanoidRobot):
         res = config[0:27] + 7 * (0., ) + config[27:34] + 7 * (0., ) + config[34:]
         return res
 
-    def __init__(self, name, initialConfig, device=None, tracer=None):
+    def __init__(self, name, initialConfig, device=None, tracer=None, fromRosParam=False):
         self.OperationalPointsMap = {
             'left-wrist': 'arm_left_7_joint',
             'right-wrist': 'arm_right_7_joint',
@@ -61,14 +63,40 @@ class Talos(AbstractHumanoidRobot):
             'chest': 'torso_2_joint'
         }
 
-        from rospkg import RosPack
-        rospack = RosPack()
-        urdfPath = rospack.get_path('talos_data') + "/urdf/talos_reduced.urdf"
-        urdfDir = [rospack.get_path('talos_data') + "/../"]
+        if fromRosParam:
+            print("Using ROS parameter \"/robot_description\"")
+            rosParamName = "/robot_description"
+            import rospy
+            if rosParamName not in rospy.get_param_names():
+                raise RuntimeError('"' + rosParamName + '" is not a ROS parameter.')
+            s = rospy.get_param(rosParamName)
+            model = buildModelFromXML(s, JointModelFreeFlyer())
+
+            # get mimic joints
+            mimicJoints = list()
+            import xml.etree.ElementTree as ET
+            root = ET.fromstring(s)
+            for e in root.iter('joint'):
+                if 'name' in e.attrib:
+                    name = e.attrib['name']
+                    for c in e._children:
+                        if hasattr(c, 'tag') and c.tag == 'mimic':
+                            mimicJoints.append(name)
+            jointIds = list()
+            for j in mimicJoints:
+                jointIds.append(model.getJointId(j))
+            q = neutral(model)
+            reducedModel = buildReducedModel(model, jointIds, q)
+            pinocchioRobot = RobotWrapper(model=reducedModel)
+        else:
+            # Create a wrapper to access the dynamic model provided
+            # through an urdf file.
+            pinocchioRobot = RobotWrapper()
+            rospack = RosPack()
+            urdfPath = rospack.get_path('talos_data') + "/urdf/" + self.defaultFilename
+            urdfDir = [rospack.get_path('talos_data') + "/../"]
+            pinocchioRobot.initFromURDF(urdfPath, urdfDir, JointModelFreeFlyer())
 
-        # Create a wrapper to access the dynamic model provided through an urdf file.
-        pinocchioRobot = RobotWrapper()
-        pinocchioRobot.initFromURDF(urdfPath, urdfDir, se3.JointModelFreeFlyer())
         self.pinocchioModel = pinocchioRobot.model
         self.pinocchioData = pinocchioRobot.data