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 -*-
# 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.)
......
......@@ -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
......
......@@ -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
......
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