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
7b437222
Unverified
Commit
7b437222
authored
May 26, 2020
by
Carlos Mastalli
Committed by
GitHub
May 26, 2020
Browse files
Merge pull request #27 from cmastalli/topic/use-pin-model
Use pinocchio model for internal functions
parents
93aa0025
f6ee3cd8
Pipeline
#9684
passed with stage
in 1 minute and 35 seconds
Changes
1
Pipelines
1
Hide whitespace changes
Inline
Side-by-side
python/example_robot_data/robots_loader.py
View file @
7b437222
...
...
@@ -27,27 +27,24 @@ def getModelPath(subpath, printmsg=False):
raise
IOError
(
'%s not found'
%
subpath
)
def
readParamsFromSrdf
(
robot
,
SRDF_PATH
,
verbose
,
has_rotor_parameters
=
True
,
referencePose
=
'half_sitting'
):
rmodel
=
robot
.
model
def
readParamsFromSrdf
(
model
,
SRDF_PATH
,
verbose
=
False
,
has_rotor_parameters
=
True
,
referencePose
=
'half_sitting'
):
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
)
pinocchio
.
loadRotorParameters
(
model
,
SRDF_PATH
,
verbose
)
model
.
armature
=
np
.
multiply
(
model
.
rotorInertia
.
flat
,
np
.
square
(
model
.
rotorGearRatio
.
flat
))
pinocchio
.
loadReferenceConfigurations
(
model
,
SRDF_PATH
,
verbose
)
q0
=
pinocchio
.
neutral
(
model
)
if
referencePose
is
not
None
:
robot
.
q0
.
flat
[:]
=
rmodel
.
referenceConfigurations
[
referencePose
].
copy
()
return
q0
=
model
.
referenceConfigurations
[
referencePose
].
copy
()
return
q0
def
addFreeFlyerJointLimits
(
robot
):
rmodel
=
robot
.
model
ub
=
rmodel
.
upperPositionLimit
def
addFreeFlyerJointLimits
(
model
):
ub
=
model
.
upperPositionLimit
ub
[:
7
]
=
1
r
model
.
upperPositionLimit
=
ub
lb
=
r
model
.
lowerPositionLimit
model
.
upperPositionLimit
=
ub
lb
=
model
.
lowerPositionLimit
lb
[:
7
]
=
-
1
r
model
.
lowerPositionLimit
=
lb
model
.
lowerPositionLimit
=
lb
def
loadANYmal
(
withArm
=
None
):
...
...
@@ -65,9 +62,9 @@ def loadANYmal(withArm=None):
# Load URDF file
robot
=
RobotWrapper
.
BuildFromURDF
(
modelPath
+
URDF_SUBPATH
,
[
modelPath
],
pinocchio
.
JointModelFreeFlyer
())
# Load SRDF file
readParamsFromSrdf
(
robot
,
modelPath
+
SRDF_SUBPATH
,
False
,
False
,
referencePose
=
REF_POSTURE
)
robot
.
q0
=
readParamsFromSrdf
(
robot
.
model
,
modelPath
+
SRDF_SUBPATH
,
False
,
False
,
referencePose
=
REF_POSTURE
)
# Add the free-flyer joint limits
addFreeFlyerJointLimits
(
robot
)
addFreeFlyerJointLimits
(
robot
.
model
)
return
robot
...
...
@@ -81,7 +78,7 @@ def loadTalosArm():
robot
=
RobotWrapper
.
BuildFromURDF
(
modelPath
+
URDF_SUBPATH
,
[
modelPath
])
# Load SRDF file
readParamsFromSrdf
(
robot
,
modelPath
+
SRDF_SUBPATH
,
False
)
robot
.
q0
=
readParamsFromSrdf
(
robot
.
model
,
modelPath
+
SRDF_SUBPATH
,
False
)
return
robot
...
...
@@ -94,10 +91,10 @@ def loadTalos():
# Load URDF file
robot
=
RobotWrapper
.
BuildFromURDF
(
modelPath
+
URDF_SUBPATH
,
[
modelPath
],
pinocchio
.
JointModelFreeFlyer
())
# Load SRDF file
readParamsFromSrdf
(
robot
,
modelPath
+
SRDF_SUBPATH
,
False
)
robot
.
q0
=
readParamsFromSrdf
(
robot
.
model
,
modelPath
+
SRDF_SUBPATH
,
False
)
assert
((
robot
.
model
.
armature
[:
6
]
==
0.
).
all
())
# Add the free-flyer joint limits
addFreeFlyerJointLimits
(
robot
)
addFreeFlyerJointLimits
(
robot
.
model
)
return
robot
...
...
@@ -155,11 +152,11 @@ def loadTalosLegs():
# Load SRDF file
robot
.
q0
=
robot
.
q0
[:
robot
.
model
.
nq
]
readParamsFromSrdf
(
robot
,
modelPath
+
SRDF_SUBPATH
,
False
)
robot
.
q0
=
readParamsFromSrdf
(
robot
.
model
,
modelPath
+
SRDF_SUBPATH
,
False
)
assert
((
m2
.
armature
[:
6
]
==
0.
).
all
())
# Add the free-flyer joint limits
addFreeFlyerJointLimits
(
robot
)
addFreeFlyerJointLimits
(
robot
.
model
)
return
robot
...
...
@@ -172,9 +169,9 @@ def loadHyQ():
# Load URDF file
robot
=
RobotWrapper
.
BuildFromURDF
(
modelPath
+
URDF_SUBPATH
,
[
modelPath
],
pinocchio
.
JointModelFreeFlyer
())
# Load SRDF file
readParamsFromSrdf
(
robot
,
modelPath
+
SRDF_SUBPATH
,
False
,
False
,
"standing"
)
robot
.
q0
=
readParamsFromSrdf
(
robot
.
model
,
modelPath
+
SRDF_SUBPATH
,
False
,
False
,
"standing"
)
# Add the free-flyer joint limits
addFreeFlyerJointLimits
(
robot
)
addFreeFlyerJointLimits
(
robot
.
model
)
return
robot
...
...
@@ -190,9 +187,9 @@ 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
,
False
,
"standing"
)
robot
.
q0
=
readParamsFromSrdf
(
robot
.
model
,
modelPath
+
SRDF_SUBPATH
,
False
,
False
,
"standing"
)
# Add the free-flyer joint limits
addFreeFlyerJointLimits
(
robot
)
addFreeFlyerJointLimits
(
robot
.
model
)
return
robot
...
...
@@ -205,7 +202,7 @@ def loadKinova():
# Load URDF file
robot
=
RobotWrapper
.
BuildFromURDF
(
modelPath
+
URDF_SUBPATH
,
[
modelPath
])
# Load SRDF file
readParamsFromSrdf
(
robot
,
modelPath
+
SRDF_SUBPATH
,
False
,
False
,
"arm_up"
)
robot
.
q0
=
readParamsFromSrdf
(
robot
.
model
,
modelPath
+
SRDF_SUBPATH
,
False
,
False
,
"arm_up"
)
return
robot
...
...
@@ -218,7 +215,7 @@ def loadTiago():
# Load URDF file
robot
=
RobotWrapper
.
BuildFromURDF
(
modelPath
+
URDF_SUBPATH
,
[
modelPath
])
# Load SRDF file
# readParamsFromSrdf(robot, modelPath+SRDF_SUBPATH, False)
#
robot.q0 =
readParamsFromSrdf(robot
.model
, modelPath+SRDF_SUBPATH, False)
return
robot
...
...
@@ -231,7 +228,7 @@ def loadTiagoNoHand():
# Load URDF file
robot
=
RobotWrapper
.
BuildFromURDF
(
modelPath
+
URDF_SUBPATH
,
[
modelPath
])
# Load SRDF file
# readParamsFromSrdf(robot, modelPath+SRDF_SUBPATH, False)
#
robot.q0 =
readParamsFromSrdf(robot
.model
, modelPath+SRDF_SUBPATH, False)
return
robot
...
...
@@ -247,9 +244,9 @@ 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
,
False
)
robot
.
q0
=
readParamsFromSrdf
(
robot
.
model
,
modelPath
+
SRDF_SUBPATH
,
False
,
False
)
# Add the free-flyer joint limits
addFreeFlyerJointLimits
(
robot
)
addFreeFlyerJointLimits
(
robot
.
model
)
return
robot
...
...
@@ -258,12 +255,12 @@ def loadUR(robot=5, limited=False, gripper=False):
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
)
model
=
RobotWrapper
.
BuildFromURDF
(
modelPath
+
URDF_SUBPATH
,
[
modelPath
])
robot
=
RobotWrapper
.
BuildFromURDF
(
modelPath
+
URDF_SUBPATH
,
[
modelPath
])
if
robot
==
5
or
robot
==
3
and
gripper
:
SRDF_FILENAME
=
"ur%i%s.srdf"
%
(
robot
,
'_gripper'
if
gripper
else
''
)
SRDF_SUBPATH
=
"/ur_description/srdf/"
+
SRDF_FILENAME
readParamsFromSrdf
(
model
,
modelPath
+
SRDF_SUBPATH
,
False
,
False
,
None
)
return
model
robot
.
q0
=
readParamsFromSrdf
(
robot
.
model
,
modelPath
+
SRDF_SUBPATH
,
False
,
False
,
None
)
return
robot
def
loadHector
():
...
...
@@ -288,9 +285,10 @@ def loadRomeo():
modelPath
=
getModelPath
(
URDF_SUBPATH
)
return
RobotWrapper
.
BuildFromURDF
(
modelPath
+
URDF_SUBPATH
,
[
modelPath
],
pinocchio
.
JointModelFreeFlyer
())
def
loadIris
():
URDF_FILENAME
=
"iris_simple.urdf"
URDF_SUBPATH
=
"/iris_description/robots/"
+
URDF_FILENAME
modelPath
=
getModelPath
(
URDF_SUBPATH
)
robot
=
RobotWrapper
.
BuildFromURDF
(
modelPath
+
URDF_SUBPATH
,
[
modelPath
],
pinocchio
.
JointModelFreeFlyer
())
return
robot
\ No newline at end of file
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