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
Humanoid Path Planner
talos-rbprm
Commits
b75e6dc4
Commit
b75e6dc4
authored
Jun 19, 2019
by
Pierre Fernbach
Browse files
[script] add python class for urdf with fixed upper body
parent
76d679c2
Changes
3
Hide whitespace changes
Inline
Side-by-side
CMakeLists.txt
View file @
b75e6dc4
...
...
@@ -51,6 +51,11 @@ install (FILES
src/hpp/corbaserver/rbprm/talos_abstract/__init__.py
DESTINATION
${
PYTHON_SITELIB
}
/hpp/corbaserver/rbprm/talos_abstract
)
install
(
FILES
src/hpp/corbaserver/rbprm/talos_fixedUpper/robot.py
src/hpp/corbaserver/rbprm/talos_fixedUpper/__init__.py
DESTINATION
${
PYTHON_SITELIB
}
/hpp/corbaserver/rbprm/talos_fixedUpper
)
install
(
FILES
src/hpp/corbaserver/rbprm/talos/robot.py
src/hpp/corbaserver/rbprm/talos/__init__.py
...
...
src/hpp/corbaserver/rbprm/talos_fixedUpper/__init__.py
0 → 100644
View file @
b75e6dc4
#!/usr/bin/env python
# Copyright (c) 2019 CNRS
# Author : Pierre Fernbach
from
robot
import
Robot
src/hpp/corbaserver/rbprm/talos_fixedUpper/robot.py
0 → 100644
View file @
b75e6dc4
#!/usr/bin/env python
# Copyright (c) 2019 CNRS
# Author: Pierre Fernbach
#
# This file is part of hpp-rbprm-robot-data.
# hpp_tutorial is free software: you can redistribute it
# and/or modify it under the terms of the GNU Lesser General Public
# License as published by the Free Software Foundation, either version
# 3 of the License, or (at your option) any later version.
#
# hpp_tutorial is distributed in the hope that it will be
# useful, but WITHOUT ANY WARRANTY; without even the implied warranty
# of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
# General Lesser Public License for more details. You should have
# received a copy of the GNU Lesser General Public License along with
# hpp_tutorial. If not, see
# <http://www.gnu.org/licenses/>.
from
hpp.corbaserver.rbprm.rbprmfullbody
import
FullBody
as
Parent
from
pinocchio
import
SE3
,
Quaternion
import
numpy
as
np
class
Robot
(
Parent
):
##
# Information to retrieve urdf and srdf files.
packageName
=
"talos_data"
meshPackageName
=
"talos_data"
rootJointType
=
"freeflyer"
urdfName
=
"talos"
urdfSuffix
=
"_reduced_fixedUpper_safeFeet"
srdfSuffix
=
""
## Information about the names of thes joints defining the limbs of the robot
rLegId
=
'talos_rleg_rom'
rleg
=
'leg_right_1_joint'
rfoot
=
'leg_right_6_joint'
lLegId
=
'talos_lleg_rom'
lleg
=
'leg_left_1_joint'
lfoot
=
'leg_left_6_joint'
referenceConfig
=
[
0.0
,
0.0
,
1.01927
,
0.0
,
0.0
,
0.0
,
1.
,
#Free flyer
0.0
,
0.0
,
-
0.411354
,
0.859395
,
-
0.448041
,
-
0.001708
,
#Left Leg
0.0
,
0.0
,
-
0.411354
,
0.859395
,
-
0.448041
,
-
0.001708
,
#Right Leg
]
referenceConfig_legsApart
=
[
0.0
,
0.0
,
1.01927
,
0.0
,
0.0
,
0.0
,
1.
,
#Free flyer
0.0
,
0.06
,
-
0.411354
,
0.859395
,
-
0.448041
,
-
0.061708
,
#Left Leg
0.0
,
-
0.06
,
-
0.411354
,
0.859395
,
-
0.448041
,
0.058292
,
#Right Leg
]
referenceConfig_armsFront
=
[
0.0
,
0.0
,
1.01927
,
0.
,
0.0
,
0.0
,
1.
,
#Free flyer
0.0
,
0.0
,
-
0.411354
,
0.859395
,
-
0.448041
,
-
0.001708
,
#Left Leg
0.0
,
0.0
,
-
0.411354
,
0.859395
,
-
0.448041
,
-
0.001708
,
#Right Leg
]
referenceConfig_legsSide
=
[
0.0
,
0.0
,
0.9832773
,
1
,
0.0
,
0.0
,
0.0
,
#Free flyer
1.57
,
0.0
,
-
0.611354
,
1.059395
,
-
0.448041
,
-
0.001708
,
#Left Leg
-
1.57
,
0.0
,
-
0.611354
,
1.059395
,
-
0.448041
,
-
0.001708
,
#Right Leg
]
postureWeights
=
[
0
,
0
,
0
,
0
,
0
,
0
,
# freeflyer
20.
,
100.
,
0.
,
0.1
,
0.
,
1.
,
#lleg
20.
,
100.
,
0.
,
0.1
,
0.
,
1.
,
#rleg
]
postureWeights_straff
=
[
0
,
0
,
0
,
0
,
0
,
0
,
# freeflyer
100.
,
1.
,
10.
,
10
,
1.
,
0.
,
#lleg
100.
,
1.
,
10.
,
10
,
1.
,
0.
,
#rleg
]
# informations required to generate the limbs databases :
nbSamples
=
50000
octreeSize
=
0.01
cType
=
"_6_DOF"
rLegOffset
=
[
0.
,
-
0.00018
,
-
0.102
]
#rLegOffset[2] += 0.006
rLegNormal
=
[
0
,
0
,
1
]
rLegx
=
0.1
;
rLegy
=
0.06
lLegOffset
=
[
0.
,
-
0.00018
,
-
0.102
]
#lLegOffset[2] += 0.006
lLegNormal
=
[
0
,
0
,
1
]
lLegx
=
0.1
;
lLegy
=
0.06
kinematicConstraintsPath
=
"package://talos-rbprm/com_inequalities/"
rLegKinematicConstraints
=
kinematicConstraintsPath
+
rleg
+
"_com_constraints.obj"
lLegKinematicConstraints
=
kinematicConstraintsPath
+
lleg
+
"_com_constraints.obj"
minDist
=
0.4
# data used by scripts :
limbs_names
=
[
rLegId
,
lLegId
]
dict_limb_rootJoint
=
{
rLegId
:
rleg
,
lLegId
:
lleg
}
dict_limb_joint
=
{
rLegId
:
rfoot
,
lLegId
:
lfoot
}
dict_limb_color_traj
=
{
rfoot
:[
0
,
1
,
0
,
1
],
lfoot
:[
1
,
0
,
0
,
1
]}
FOOT_SAFETY_SIZE
=
0.03
# size of the contact surface (x,y)
dict_size
=
{
rfoot
:[
0.2
,
0.13
],
lfoot
:[
0.2
,
0.13
]}
# various offset used by scripts :
MRsole_offset
=
SE3
.
Identity
()
MRsole_offset
.
translation
=
np
.
matrix
(
rLegOffset
).
T
MLsole_offset
=
SE3
.
Identity
()
MLsole_offset
.
translation
=
np
.
matrix
(
lLegOffset
).
T
dict_offset
=
{
rfoot
:
MRsole_offset
,
lfoot
:
MLsole_offset
}
dict_normal
=
{
rfoot
:
rLegNormal
,
lfoot
:
lLegNormal
}
# display transform :
#MRsole_display = MRsole_offset.copy()
#MLsole_display = MLsole_offset.copy()
MRsole_display
=
SE3
.
Identity
()
MLsole_display
=
SE3
.
Identity
()
dict_display_offset
=
{
rfoot
:
MRsole_display
,
lfoot
:
MLsole_display
}
kneeIds
=
{
"Left"
:
10
,
"Right"
:
16
}
def
__init__
(
self
,
name
=
None
,
load
=
True
):
Parent
.
__init__
(
self
,
load
)
if
load
:
self
.
loadFullBodyModel
(
self
.
urdfName
,
self
.
rootJointType
,
self
.
meshPackageName
,
self
.
packageName
,
self
.
urdfSuffix
,
self
.
srdfSuffix
)
if
name
!=
None
:
self
.
name
=
name
self
.
joint1L_bounds_prev
=
self
.
getJointBounds
(
'leg_left_1_joint'
)
self
.
joint6L_bounds_prev
=
self
.
getJointBounds
(
'leg_left_6_joint'
)
self
.
joint2L_bounds_prev
=
self
.
getJointBounds
(
'leg_left_2_joint'
)
self
.
joint1R_bounds_prev
=
self
.
getJointBounds
(
'leg_right_1_joint'
)
self
.
joint6R_bounds_prev
=
self
.
getJointBounds
(
'leg_right_6_joint'
)
self
.
joint2R_bounds_prev
=
self
.
getJointBounds
(
'leg_right_2_joint'
)
def
loadAllLimbs
(
self
,
heuristic
,
analysis
=
None
,
nbSamples
=
nbSamples
,
octreeSize
=
octreeSize
):
for
id
in
self
.
limbs_names
:
eff
=
self
.
dict_limb_joint
[
id
]
self
.
addLimb
(
id
,
self
.
dict_limb_rootJoint
[
id
],
eff
,
self
.
dict_offset
[
eff
].
translation
.
T
.
tolist
()[
0
],
self
.
dict_normal
[
eff
],
self
.
dict_size
[
eff
][
0
]
/
2.
,
self
.
dict_size
[
eff
][
1
]
/
2.
,
nbSamples
,
heuristic
,
octreeSize
,
self
.
cType
,
kinematicConstraintsPath
=
self
.
kinematicConstraintsPath
+
self
.
dict_limb_rootJoint
[
id
]
+
"_com_constraints.obj"
,
kinematicConstraintsMin
=
self
.
minDist
)
if
analysis
:
self
.
runLimbSampleAnalysis
(
id
,
analysis
,
True
)
def
setConstrainedJointsBounds
(
self
):
self
.
setJointBounds
(
'leg_left_1_joint'
,[
-
0.34
,
1.4
])
self
.
setJointBounds
(
'leg_left_6_joint'
,[
-
0.25
,
0.25
])
self
.
setJointBounds
(
'leg_left_2_joint'
,[
-
0.25
,
0.25
])
self
.
setJointBounds
(
'leg_right_1_joint'
,[
-
1.4
,
0.34
])
self
.
setJointBounds
(
'leg_right_6_joint'
,[
-
0.25
,
0.25
])
self
.
setJointBounds
(
'leg_right_2_joint'
,[
-
0.25
,
0.25
])
def
resetJointsBounds
(
self
):
self
.
setJointBounds
(
'leg_left_1_joint'
,
self
.
joint1L_bounds_prev
)
self
.
setJointBounds
(
'leg_left_6_joint'
,
self
.
joint6L_bounds_prev
)
self
.
setJointBounds
(
'leg_left_2_joint'
,
self
.
joint2L_bounds_prev
)
self
.
setJointBounds
(
'leg_right_1_joint'
,
self
.
joint1R_bounds_prev
)
self
.
setJointBounds
(
'leg_right_6_joint'
,
self
.
joint6R_bounds_prev
)
self
.
setJointBounds
(
'leg_right_2_joint'
,
self
.
joint2R_bounds_prev
)
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