Skip to content
GitLab
Menu
Projects
Groups
Snippets
Help
Help
Support
Community forum
Keyboard shortcuts
?
Submit feedback
Contribute to GitLab
Sign in / Register
Toggle navigation
Menu
Open sidebar
Stack Of Tasks
sot-talos
Commits
a2553a3f
Commit
a2553a3f
authored
Nov 28, 2019
by
Guilhem Saurel
Browse files
Merge remote-tracking branch 'main/devel' into devel
parents
bc826e50
47403bb0
Pipeline
#7288
failed with stage
in 40 seconds
Changes
3
Pipelines
1
Hide whitespace changes
Inline
Side-by-side
src/dynamic_graph/sot/pyrene/prologue.py
View file @
a2553a3f
# -*- 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
View file @
a2553a3f
...
...
@@ -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
View file @
a2553a3f
...
...
@@ -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
...
...
Write
Preview
Supports
Markdown
0%
Try again
or
attach a new file
.
Attach a file
Cancel
You are about to add
0
people
to the discussion. Proceed with caution.
Finish editing this message first!
Cancel
Please
register
or
sign in
to comment