Skip to content
GitLab
Projects
Groups
Snippets
/
Help
Help
Support
Community forum
Keyboard shortcuts
?
Submit feedback
Contribute to GitLab
Sign in / Register
Toggle navigation
Menu
Open sidebar
Carlos Mastalli
example-robot-data
Commits
8a2da0df
Commit
8a2da0df
authored
Nov 13, 2019
by
Pep Marti Saumell
Browse files
Cmake fixes and formatting
parent
a6279e8b
Changes
4
Hide whitespace changes
Inline
Side-by-side
CMakeLists.txt
View file @
8a2da0df
...
...
@@ -49,6 +49,7 @@ IF(NOT INSTALL_PYTHON_INTERFACE_ONLY)
INSTALL
(
DIRECTORY kinova_description DESTINATION share/
${
PROJECT_NAME
}
)
INSTALL
(
DIRECTORY tiago_description DESTINATION share/
${
PROJECT_NAME
}
)
INSTALL
(
DIRECTORY ur_description DESTINATION share/
${
PROJECT_NAME
}
)
INSTALL
(
DIRECTORY romeo_description DESTINATION share/
${
PROJECT_NAME
}
)
INSTALL
(
DIRECTORY hector_description DESTINATION share/
${
PROJECT_NAME
}
)
INSTALL
(
DIRECTORY double_pendulum_description DESTINATION share/
${
PROJECT_NAME
}
)
ENDIF
(
NOT INSTALL_PYTHON_INTERFACE_ONLY
)
hector_description/meshes/quadrotor/blender/quadrotor_base.blend
deleted
100644 → 0
View file @
a6279e8b
File deleted
python/example_robot_data/__main__.py
View file @
8a2da0df
...
...
@@ -3,8 +3,9 @@ from argparse import ArgumentParser
from
.
import
robots_loader
ROBOTS
=
[
'anymal'
,
'anymal_kinova'
,
'hyq'
,
'solo'
,
'solo12'
,
'talos'
,
'talos_arm'
,
'talos_legs'
,
'kinova'
,
'tiago'
,
'tiago_no_hand'
,
'icub'
,
'ur5'
,
'romeo'
,
'hector'
,
'double_pendulum'
'anymal'
,
'anymal_kinova'
,
'hyq'
,
'solo'
,
'solo12'
,
'talos'
,
'talos_arm'
,
'talos_legs'
,
'kinova'
,
'tiago'
,
'tiago_no_hand'
,
'icub'
,
'ur5'
,
'romeo'
,
'hector'
,
'double_pendulum'
]
parser
=
ArgumentParser
()
...
...
@@ -76,7 +77,7 @@ elif args.robot == 'ur5':
ur5
=
robots_loader
.
loadUR
()
ur5
.
initViewer
(
loadModel
=
True
)
ur5
.
display
(
ur5
.
q0
)
elif
args
.
robot
==
'romeo'
:
romeo
=
robots_loader
.
loadRomeo
()
romeo
.
initViewer
(
loadModel
=
True
)
...
...
@@ -91,4 +92,3 @@ if args.robot == 'double_pendulum':
planar2dof
=
robots_loader
.
load2dof
()
planar2dof
.
initViewer
(
loadModel
=
True
)
planar2dof
.
display
(
planar2dof
.
q0
)
python/example_robot_data/robots_loader.py
View file @
8a2da0df
...
...
@@ -8,8 +8,10 @@ from pinocchio.robot_wrapper import RobotWrapper
def
getModelPath
(
subpath
,
printmsg
=
False
):
base
=
'../../../share/example-robot-data'
for
path
in
[
dirname
(
dirname
(
dirname
(
dirname
(
__file__
)))),
dirname
(
dirname
(
dirname
(
__file__
)))]
+
[
join
(
p
,
base
.
strip
(
'/'
))
for
p
in
sys
.
path
]:
for
path
in
[
dirname
(
dirname
(
dirname
(
dirname
(
__file__
)))),
dirname
(
dirname
(
dirname
(
__file__
)))
]
+
[
join
(
p
,
base
.
strip
(
'/'
))
for
p
in
sys
.
path
]:
if
exists
(
join
(
path
,
subpath
.
strip
(
'/'
))):
if
printmsg
:
print
(
"using %s as modelPath"
%
path
)
...
...
@@ -17,12 +19,17 @@ def getModelPath(subpath, printmsg=False):
raise
IOError
(
'%s not found'
%
(
subpath
))
def
readParamsFromSrdf
(
robot
,
SRDF_PATH
,
verbose
,
has_rotor_parameters
=
True
,
referencePose
=
'half_sitting'
):
def
readParamsFromSrdf
(
robot
,
SRDF_PATH
,
verbose
,
has_rotor_parameters
=
True
,
referencePose
=
'half_sitting'
):
rmodel
=
robot
.
model
if
has_rotor_parameters
:
pinocchio
.
loadRotorParameters
(
rmodel
,
SRDF_PATH
,
verbose
)
rmodel
.
armature
=
np
.
multiply
(
rmodel
.
rotorInertia
.
flat
,
np
.
square
(
rmodel
.
rotorGearRatio
.
flat
))
rmodel
.
armature
=
np
.
multiply
(
rmodel
.
rotorInertia
.
flat
,
np
.
square
(
rmodel
.
rotorGearRatio
.
flat
))
pinocchio
.
loadReferenceConfigurations
(
rmodel
,
SRDF_PATH
,
verbose
)
robot
.
q0
.
flat
[:]
=
rmodel
.
referenceConfigurations
[
referencePose
].
copy
()
return
...
...
@@ -52,9 +59,14 @@ def loadANYmal(withArm=None):
SRDF_SUBPATH
=
"/anymal_b_simple_description/srdf/"
+
SRDF_FILENAME
modelPath
=
getModelPath
(
URDF_SUBPATH
)
# Load URDF file
robot
=
RobotWrapper
.
BuildFromURDF
(
modelPath
+
URDF_SUBPATH
,
[
modelPath
],
pinocchio
.
JointModelFreeFlyer
())
robot
=
RobotWrapper
.
BuildFromURDF
(
modelPath
+
URDF_SUBPATH
,
[
modelPath
],
pinocchio
.
JointModelFreeFlyer
())
# Load SRDF file
readParamsFromSrdf
(
robot
,
modelPath
+
SRDF_SUBPATH
,
False
,
False
,
referencePose
=
REF_POSTURE
)
readParamsFromSrdf
(
robot
,
modelPath
+
SRDF_SUBPATH
,
False
,
False
,
referencePose
=
REF_POSTURE
)
# Add the free-flyer joint limits
addFreeFlyerJointLimits
(
robot
)
return
robot
...
...
@@ -81,7 +93,8 @@ def loadTalos():
URDF_SUBPATH
=
"/talos_data/robots/"
+
URDF_FILENAME
modelPath
=
getModelPath
(
URDF_SUBPATH
)
# Load URDF file
robot
=
RobotWrapper
.
BuildFromURDF
(
modelPath
+
URDF_SUBPATH
,
[
modelPath
],
pinocchio
.
JointModelFreeFlyer
())
robot
=
RobotWrapper
.
BuildFromURDF
(
modelPath
+
URDF_SUBPATH
,
[
modelPath
],
pinocchio
.
JointModelFreeFlyer
())
# Load SRDF file
readParamsFromSrdf
(
robot
,
modelPath
+
SRDF_SUBPATH
,
False
)
assert
((
robot
.
model
.
armature
[:
6
]
==
0.
).
all
())
...
...
@@ -101,13 +114,17 @@ def loadTalosLegs():
legMaxId
=
14
m1
=
robot
.
model
m2
=
pinocchio
.
Model
()
for
j
,
M
,
name
,
parent
,
Y
in
zip
(
m1
.
joints
,
m1
.
jointPlacements
,
m1
.
names
,
m1
.
parents
,
m1
.
inertias
):
for
j
,
M
,
name
,
parent
,
Y
in
zip
(
m1
.
joints
,
m1
.
jointPlacements
,
m1
.
names
,
m1
.
parents
,
m1
.
inertias
):
if
j
.
id
<
legMaxId
:
jid
=
m2
.
addJoint
(
parent
,
getattr
(
pinocchio
,
j
.
shortname
())(),
M
,
name
)
jid
=
m2
.
addJoint
(
parent
,
getattr
(
pinocchio
,
j
.
shortname
())(),
M
,
name
)
up
=
m2
.
upperPositionLimit
down
=
m2
.
lowerPositionLimit
up
[
m2
.
joints
[
jid
].
idx_q
:
m2
.
joints
[
jid
].
idx_q
+
j
.
nq
]
=
m1
.
upperPositionLimit
[
j
.
idx_q
:
j
.
idx_q
+
j
.
nq
]
down
[
m2
.
joints
[
jid
].
idx_q
:
m2
.
joints
[
jid
].
idx_q
+
j
.
nq
]
=
m1
.
lowerPositionLimit
[
j
.
idx_q
:
j
.
idx_q
+
j
.
nq
]
up
[
m2
.
joints
[
jid
].
idx_q
:
m2
.
joints
[
jid
].
idx_q
+
j
.
nq
]
=
m1
.
upperPositionLimit
[
j
.
idx_q
:
j
.
idx_q
+
j
.
nq
]
down
[
m2
.
joints
[
jid
].
idx_q
:
m2
.
joints
[
jid
].
idx_q
+
j
.
nq
]
=
m1
.
lowerPositionLimit
[
j
.
idx_q
:
j
.
idx_q
+
j
.
nq
]
m2
.
upperPositionLimit
=
up
m2
.
lowerPositionLimit
=
down
assert
(
jid
==
j
.
id
)
...
...
@@ -153,9 +170,11 @@ def loadHyQ():
URDF_SUBPATH
=
"/hyq_description/robots/"
+
URDF_FILENAME
modelPath
=
getModelPath
(
URDF_SUBPATH
)
# Load URDF file
robot
=
RobotWrapper
.
BuildFromURDF
(
modelPath
+
URDF_SUBPATH
,
[
modelPath
],
pinocchio
.
JointModelFreeFlyer
())
robot
=
RobotWrapper
.
BuildFromURDF
(
modelPath
+
URDF_SUBPATH
,
[
modelPath
],
pinocchio
.
JointModelFreeFlyer
())
# Load SRDF file
readParamsFromSrdf
(
robot
,
modelPath
+
SRDF_SUBPATH
,
False
,
False
,
"standing"
)
readParamsFromSrdf
(
robot
,
modelPath
+
SRDF_SUBPATH
,
False
,
False
,
"standing"
)
# Add the free-flyer joint limits
addFreeFlyerJointLimits
(
robot
)
return
robot
...
...
@@ -171,9 +190,11 @@ def loadSolo(solo=True):
URDF_SUBPATH
=
"/solo_description/robots/"
+
URDF_FILENAME
modelPath
=
getModelPath
(
URDF_SUBPATH
)
# Load URDF file
robot
=
RobotWrapper
.
BuildFromURDF
(
modelPath
+
URDF_SUBPATH
,
[
modelPath
],
pinocchio
.
JointModelFreeFlyer
())
robot
=
RobotWrapper
.
BuildFromURDF
(
modelPath
+
URDF_SUBPATH
,
[
modelPath
],
pinocchio
.
JointModelFreeFlyer
())
# Load SRDF file
readParamsFromSrdf
(
robot
,
modelPath
+
SRDF_SUBPATH
,
False
,
False
,
"standing"
)
readParamsFromSrdf
(
robot
,
modelPath
+
SRDF_SUBPATH
,
False
,
False
,
"standing"
)
# Add the free-flyer joint limits
addFreeFlyerJointLimits
(
robot
)
return
robot
...
...
@@ -228,7 +249,8 @@ def loadICub(reduced=True):
URDF_SUBPATH
=
"/icub_description/robots/"
+
URDF_FILENAME
modelPath
=
getModelPath
(
URDF_SUBPATH
)
# Load URDF file
robot
=
RobotWrapper
.
BuildFromURDF
(
modelPath
+
URDF_SUBPATH
,
[
modelPath
],
pinocchio
.
JointModelFreeFlyer
())
robot
=
RobotWrapper
.
BuildFromURDF
(
modelPath
+
URDF_SUBPATH
,
[
modelPath
],
pinocchio
.
JointModelFreeFlyer
())
# Load SRDF file
readParamsFromSrdf
(
robot
,
modelPath
+
SRDF_SUBPATH
,
False
,
False
)
# Add the free-flyer joint limits
...
...
@@ -238,7 +260,8 @@ def loadICub(reduced=True):
def
loadUR
(
robot
=
5
,
limited
=
False
,
gripper
=
False
):
assert
(
not
(
gripper
and
(
robot
==
10
or
limited
)))
URDF_FILENAME
=
"ur%i%s_%s.urdf"
%
(
robot
,
"_joint_limited"
if
limited
else
''
,
'gripper'
if
gripper
else
'robot'
)
URDF_FILENAME
=
"ur%i%s_%s.urdf"
%
(
robot
,
"_joint_limited"
if
limited
else
''
,
'gripper'
if
gripper
else
'robot'
)
URDF_SUBPATH
=
"/ur_description/urdf/"
+
URDF_FILENAME
modelPath
=
getModelPath
(
URDF_SUBPATH
)
return
RobotWrapper
.
BuildFromURDF
(
modelPath
+
URDF_SUBPATH
,
[
modelPath
])
...
...
@@ -248,7 +271,8 @@ def loadHector():
URDF_FILENAME
=
"quadrotor_base.urdf"
URDF_SUBPATH
=
"/hector_description/robots/"
+
URDF_FILENAME
modelPath
=
getModelPath
(
URDF_SUBPATH
)
robot
=
RobotWrapper
.
BuildFromURDF
(
modelPath
+
URDF_SUBPATH
,
[
modelPath
],
pinocchio
.
JointModelFreeFlyer
())
robot
=
RobotWrapper
.
BuildFromURDF
(
modelPath
+
URDF_SUBPATH
,
[
modelPath
],
pinocchio
.
JointModelFreeFlyer
())
return
robot
...
...
@@ -258,9 +282,11 @@ def loadDoublePendulum():
modelPath
=
getModelPath
(
URDF_SUBPATH
)
robot
=
RobotWrapper
.
BuildFromURDF
(
modelPath
+
URDF_SUBPATH
,
[
modelPath
])
return
robot
def
loadRomeo
():
URDF_FILENAME
=
"romeo.urdf"
URDF_SUBPATH
=
"/romeo_description/urdf/"
+
URDF_FILENAME
modelPath
=
getModelPath
(
URDF_SUBPATH
)
return
RobotWrapper
.
BuildFromURDF
(
modelPath
+
URDF_SUBPATH
,
[
modelPath
],
pinocchio
.
JointModelFreeFlyer
())
return
RobotWrapper
.
BuildFromURDF
(
modelPath
+
URDF_SUBPATH
,
[
modelPath
],
pinocchio
.
JointModelFreeFlyer
())
Write
Preview
Supports
Markdown
0%
Try again
or
attach a new 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