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
Gepetto
example-robot-data
Commits
6e02372e
Commit
6e02372e
authored
Sep 23, 2019
by
Guilhem Saurel
Browse files
some models don't have robot parameters, fix #13
parent
ec0673d7
Changes
1
Hide whitespace changes
Inline
Side-by-side
example_robot_data/robots_loader.py
View file @
6e02372e
...
...
@@ -2,6 +2,7 @@ import sys
from
os.path
import
dirname
,
exists
,
join
import
numpy
as
np
import
pinocchio
from
pinocchio.robot_wrapper
import
RobotWrapper
...
...
@@ -22,10 +23,11 @@ def getModelPath(subpath, printmsg=False):
raise
IOError
(
'%s not found'
%
(
subpath
))
def
readParamsFromSrdf
(
robot
,
SRDF_PATH
,
verbose
):
def
readParamsFromSrdf
(
robot
,
SRDF_PATH
,
verbose
,
has_rotor_parameters
=
True
):
rmodel
=
robot
.
model
pinocchio
.
loadRotorParameters
(
rmodel
,
SRDF_PATH
,
verbose
)
if
has_rotor_parameters
:
pinocchio
.
loadRotorParameters
(
rmodel
,
SRDF_PATH
,
verbose
)
rmodel
.
armature
=
np
.
multiply
(
rmodel
.
rotorInertia
.
flat
,
np
.
square
(
rmodel
.
rotorGearRatio
.
flat
))
pinocchio
.
loadReferenceConfigurations
(
rmodel
,
SRDF_PATH
,
verbose
)
robot
.
q0
.
flat
[:]
=
rmodel
.
referenceConfigurations
[
"half_sitting"
].
copy
()
...
...
@@ -138,7 +140,7 @@ def loadHyQ():
# Load URDF file
robot
=
RobotWrapper
.
BuildFromURDF
(
modelPath
+
URDF_SUBPATH
,
[
modelPath
],
pinocchio
.
JointModelFreeFlyer
())
# Load SRDF file
readParamsFromSrdf
(
robot
,
modelPath
+
SRDF_SUBPATH
,
False
)
readParamsFromSrdf
(
robot
,
modelPath
+
SRDF_SUBPATH
,
False
,
False
)
# Add the free-flyer joint limits
addFreeFlyerJointLimits
(
robot
)
return
robot
...
...
@@ -156,7 +158,7 @@ def loadSolo(solo=True):
# Load URDF file
robot
=
RobotWrapper
.
BuildFromURDF
(
modelPath
+
URDF_SUBPATH
,
[
modelPath
],
pinocchio
.
JointModelFreeFlyer
())
# Load SRDF file
readParamsFromSrdf
(
robot
,
modelPath
+
SRDF_SUBPATH
,
False
)
readParamsFromSrdf
(
robot
,
modelPath
+
SRDF_SUBPATH
,
False
,
False
)
# Add the free-flyer joint limits
addFreeFlyerJointLimits
(
robot
)
return
robot
...
...
@@ -200,7 +202,7 @@ def loadICub(reduced=True):
# Load URDF file
robot
=
RobotWrapper
.
BuildFromURDF
(
modelPath
+
URDF_SUBPATH
,
[
modelPath
],
pinocchio
.
JointModelFreeFlyer
())
# Load SRDF file
readParamsFromSrdf
(
robot
,
modelPath
+
SRDF_SUBPATH
,
False
)
readParamsFromSrdf
(
robot
,
modelPath
+
SRDF_SUBPATH
,
False
,
False
)
# Add the free-flyer joint limits
addFreeFlyerJointLimits
(
robot
)
return
robot
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