Commit a2553a3f authored by Guilhem Saurel's avatar Guilhem Saurel

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

parents bc826e50 47403bb0
Pipeline #7288 failed with stage
in 40 seconds
# -*- coding: utf-8 -*- # -*- coding: utf-8 -*-
# Copyright 2011, Florent Lamiraux, Thomas Moulard, JRL, CNRS/AIST # 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.entity import PyEntityFactoryClass
from dynamic_graph.sot.pyrene.robot import Robot from dynamic_graph.sot.pyrene.robot import Robot
if not hasattr(sys, 'argv'):
sys.argv = [
"dynamic_graph",
]
print("Prologue Pyrene TALOS Robot") print("Prologue Pyrene TALOS Robot")
...@@ -19,8 +28,7 @@ def makeRobot(): ...@@ -19,8 +28,7 @@ def makeRobot():
DeviceTalos = PyEntityFactoryClass('DeviceTalos') DeviceTalos = PyEntityFactoryClass('DeviceTalos')
# Create the robot using the device. # 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) robot.dynamic.com.recompute(0)
_com = robot.dynamic.com.value _com = robot.dynamic.com.value
robot.device.zmp.value = (_com[0], _com[1], 0.) robot.device.zmp.value = (_com[0], _com[1], 0.)
......
...@@ -50,7 +50,7 @@ class Robot(Talos): ...@@ -50,7 +50,7 @@ class Robot(Talos):
0. # Head 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) Talos.__init__(self, name, self.halfSitting, device, tracer)
""" """
TODO:Confirm these values TODO:Confirm these values
......
...@@ -3,12 +3,13 @@ ...@@ -3,12 +3,13 @@
from __future__ import print_function from __future__ import print_function
import pinocchio as se3
from dynamic_graph import plug from dynamic_graph import plug
from dynamic_graph.sot.core.math_small_entities import Derivator_of_Vector 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 import DynamicPinocchio
from dynamic_graph.sot.dynamics_pinocchio.humanoid_robot import AbstractHumanoidRobot from dynamic_graph.sot.dynamics_pinocchio.humanoid_robot import AbstractHumanoidRobot
from pinocchio import JointModelFreeFlyer, buildModelFromXML, buildReducedModel, neutral
from pinocchio.robot_wrapper import RobotWrapper from pinocchio.robot_wrapper import RobotWrapper
from rospkg import RosPack
# Internal helper tool. # Internal helper tool.
...@@ -27,6 +28,7 @@ class Talos(AbstractHumanoidRobot): ...@@ -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.)) 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.)) 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 TODO: Confirm the position and existence of these sensors
accelerometerPosition = np.matrix (( accelerometerPosition = np.matrix ((
...@@ -50,7 +52,7 @@ class Talos(AbstractHumanoidRobot): ...@@ -50,7 +52,7 @@ class Talos(AbstractHumanoidRobot):
res = config[0:27] + 7 * (0., ) + config[27:34] + 7 * (0., ) + config[34:] res = config[0:27] + 7 * (0., ) + config[27:34] + 7 * (0., ) + config[34:]
return res return res
def __init__(self, name, initialConfig, device=None, tracer=None): def __init__(self, name, initialConfig, device=None, tracer=None, fromRosParam=False):
self.OperationalPointsMap = { self.OperationalPointsMap = {
'left-wrist': 'arm_left_7_joint', 'left-wrist': 'arm_left_7_joint',
'right-wrist': 'arm_right_7_joint', 'right-wrist': 'arm_right_7_joint',
...@@ -61,14 +63,40 @@ class Talos(AbstractHumanoidRobot): ...@@ -61,14 +63,40 @@ class Talos(AbstractHumanoidRobot):
'chest': 'torso_2_joint' 'chest': 'torso_2_joint'
} }
from rospkg import RosPack if fromRosParam:
rospack = RosPack() print("Using ROS parameter \"/robot_description\"")
urdfPath = rospack.get_path('talos_data') + "/urdf/talos_reduced.urdf" rosParamName = "/robot_description"
urdfDir = [rospack.get_path('talos_data') + "/../"] 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.pinocchioModel = pinocchioRobot.model
self.pinocchioData = pinocchioRobot.data self.pinocchioData = pinocchioRobot.data
......
Markdown is supported
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment