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
Guilhem Saurel
hpp-rbprm-corba
Commits
2ecd7a4d
Unverified
Commit
2ecd7a4d
authored
Sep 02, 2020
by
Fernbach Pierre
Committed by
GitHub
Sep 02, 2020
Browse files
Merge pull request #73 from pFernbach/devel
Update API to load robots
parents
e7d6ad6c
c3fe7e60
Changes
24
Show whitespace changes
Inline
Side-by-side
idl/hpp/corbaserver/rbprm/rbprmbuilder.idl
View file @
2ecd7a4d
...
...
@@ -32,21 +32,14 @@ module hpp
///
This
function
can
be
called
several
times
to
include
several
ROMs
(
one
for
each
limb
)
///
The
device
automatically
has
an
anchor
joint
called
"
base_joint
"
as
///
root
joint
.
///
\
param
romRobotName
the
name
of
the
robot
range
of
motion
.
///
Load
robot
model
///
\
param
robotName
the
name
of
the
robot
range
of
motion
,
///
\
param
rootJointType
type
of
root
joint
among
"anchor"
,
"freeflyer"
,
///
"planar"
,
///
\
param
packageName
Name
of
the
ROS
package
containing
the
model
,
///
\
param
modelName
Name
of
the
package
containing
the
model
///
\
param
urdfSuffix
suffix
for
urdf
file
,
///
\
param
urdfName
name
of
the
urdf
file
.
It
may
contain
///
"file://"
,
or
"package://"
prefixes
.
///
///
The
ros
url
are
built
as
follows
:
///
"package://${packageName}/urdf/${modelName}${urdfSuffix}.urdf"
///
"package://${packageName}/srdf/${modelName}${srdfSuffix}.srdf"
///
void
loadRobotRomModel
(
in
string
romRobotName
,
in
string
rootJointType
,
in
string
packageName
,
in
string
modelName
,
in
string
urdfSuffix
,
in
string
srdfSuffix
)
void
loadRobotRomModel
(
in
string
robotName
,
in
string
rootJointType
,
in
string
urdfName
)
raises
(
Error
)
;
...
...
@@ -54,39 +47,34 @@ module hpp
///
The
device
automatically
has
an
anchor
joint
called
"base_joint"
as
///
root
joint
.
///
\
param
trunkR
obotName
the
name
of
the
robot
trunk
used
for
collision
.
///
\
param
r
obotName
the
name
of
the
robot
trunk
used
for
collision
,
///
\
param
rootJointType
type
of
root
joint
among
"anchor"
,
"freeflyer"
,
///
"planar"
,
///
\
param
packageName
Name
of
the
ROS
package
containing
the
model
,
///
\
param
modelName
Name
of
the
package
containing
the
model
///
\
param
urdfSuffix
suffix
for
urdf
file
,
///
///
The
ros
url
are
built
as
follows
:
///
"package://${packageName}/urdf/${modelName}${urdfSuffix}.urdf"
///
"package://${packageName}/srdf/${modelName}${srdfSuffix}.srdf"
///
\
param
urdfName
name
of
the
urdf
file
.
It
may
contain
///
"file://"
,
or
"package://"
prefixes
.
///
\
param
srdfName
name
of
the
srdf
file
.
It
may
contain
///
"file://"
,
or
"package://"
prefixes
.
///
void
loadRobotCompleteModel
(
in
string
trunkRobotName
,
in
string
rootJointType
,
in
string
packageName
,
in
string
modelName
,
in
string
urdfSuffix
,
in
string
srdfSuffix
)
void
loadRobotCompleteModel
(
in
string
robotName
,
in
string
rootJointType
,
in
string
urdfName
,
in
string
srdfName
)
raises
(
Error
)
;
///
Load
fullbody
robot
model
///
///
Create
a
RbprmFullBody
object
///
The
device
automatically
has
an
anchor
joint
called
"base_joint"
as
///
root
joint
.
///
\
param
trunkRobotName
the
name
of
the
robot
trunk
used
for
collision
.
///
///
\
param
robotName
Name
of
the
robot
that
is
constructed
,
///
\
param
rootJointType
type
of
root
joint
among
"anchor"
,
"freeflyer"
,
///
"planar"
,
///
\
param
packageName
Name
of
the
ROS
package
containing
the
model
,
///
\
param
modelName
Name
of
the
package
containing
the
model
///
\
param
urdfSuffix
suffix
for
urdf
file
,
///
///
The
ros
url
are
built
as
follows
:
///
"package://${packageName}/urdf/${modelName}${urdfSuffix}.urdf"
///
"package://${packageName}/srdf/${modelName}${srdfSuffix}.srdf"
///
\
param
urdfName
name
of
the
urdf
file
.
It
may
contain
///
"file://"
,
or
"package://"
prefixes
.
///
\
param
srdfName
name
of
the
srdf
file
.
It
may
contain
///
"file://"
,
or
"package://"
prefixes
.
///
void
loadFullBodyRobot
(
in
string
trunkRobotName
,
in
string
rootJointType
,
in
string
packageName
,
in
string
modelName
,
in
string
urdfSuffix
,
in
string
srdfSuffix
,
in
string
selectedProblem
)
void
loadFullBodyRobot
(
in
string
robotName
,
in
string
rootJointType
,
in
string
urdfName
,
in
string
srdfName
,
in
string
selectedProblem
)
raises
(
Error
)
;
///
Create
a
RbprmFullBody
object
...
...
matlab/effectorRomToObj.m
0 → 100755
View file @
2ecd7a4d
% Open it on blender with default axis setting and export it with x forward.
% Use decimate to reduce the number of faces, don't forget to triangulate faces before exporting
function
[]
=
effectorRomToObj
(
filename
,
outname
)
delimiterIn
=
','
;
headerlinesIn
=
0
;
points
=
importdata
(
filename
,
delimiterIn
,
headerlinesIn
);
k
=
convhull
(
points
);
vertface2obj
(
points
,
k
,
outname
)
matlab/vertface2obj.m
0 → 100755
View file @
2ecd7a4d
function
vertface2obj
(
v
,
f
,
name
)
% VERTFACE2OBJ Save a set of vertice coordinates and faces as a Wavefront/Alias Obj file
% VERTFACE2OBJ(v,f,fname)
% v is a Nx3 matrix of vertex coordinates.
% f is a Mx3 matrix of vertex indices.
% fname is the filename to save the obj file.
fid
=
fopen
(
name
,
'w'
);
for
i
=
1
:
size
(
v
,
1
)
fprintf
(
fid
,
'v %f %f %f\n'
,
v
(
i
,
1
),
v
(
i
,
2
),
v
(
i
,
3
));
end
fprintf
(
fid
,
'g foo\n'
);
for
i
=
1
:
size
(
f
,
1
);
fprintf
(
fid
,
'f %d %d %d\n'
,
f
(
i
,
1
),
f
(
i
,
2
),
f
(
i
,
3
));
end
fprintf
(
fid
,
'g\n'
);
fclose
(
fid
);
src/CMakeLists.txt
View file @
2ecd7a4d
...
...
@@ -88,6 +88,7 @@ SET(${PROJECT_NAME}_PYTHON_FILES
SET
(
${
PROJECT_NAME
}
_PYTHON_TOOLS
affordance_centroids.py
com_constraints.py
constants_and_tools.py
constraint_to_dae.py
cwc_trajectory_helper.py
cwc_trajectory.py
...
...
src/hpp/corbaserver/rbprm/rbprmbuilder.py
View file @
2ecd7a4d
...
...
@@ -26,42 +26,55 @@ from hpp.corbaserver.robot import Robot
# trunk of the robot, and a set of robots describe the range of motion of each limb of the robot.
class
Builder
(
Robot
):
# # Constructor
def
__init__
(
self
,
load
=
True
,
clientRbprm
=
None
):
def
__init__
(
self
,
robotName
=
None
,
rootJointType
=
None
,
load
=
True
,
client
=
None
,
hppcorbaClient
=
None
,
clientRbprm
=
None
):
Robot
.
__init__
(
self
,
robotName
,
rootJointType
,
False
,
client
,
hppcorbaClient
)
self
.
tf_root
=
"base_link"
if
clientRbprm
is
None
:
self
.
clientRbprm
=
RbprmClient
()
else
:
self
.
clientRbprm
=
clientRbprm
self
.
load
=
load
if
load
:
self
.
loadModel
(
robotName
,
rootJointType
)
## Return urdf and srdf filenames of the roms
#
def
urdfROMFilenames
(
self
):
urdfNameRoms
=
[]
nameRoms
=
[]
if
hasattr
(
self
,
'packageName'
)
and
hasattr
(
self
,
'urdfNameRom'
)
and
\
hasattr
(
self
,
'urdfSuffix'
)
:
if
not
isinstance
(
self
.
urdfNameRom
,
list
):
self
.
urdfNameRom
=
[
self
.
urdfNameRom
]
nameRoms
=
self
.
urdfNameRom
for
urdfName
in
nameRoms
:
urdfNameRoms
+=
[
"package://"
+
self
.
packageName
+
'/urdf/'
+
urdfName
+
self
.
urdfSuffix
+
'.urdf'
]
elif
hasattr
(
self
,
'urdfFilenameRom'
):
urdfNameRoms
=
self
.
urdfFilenameRom
for
urdfNameRom
in
urdfNameRoms
:
nameRoms
+=
[
urdfNameRom
.
split
(
"/"
)[
-
1
].
rstrip
(
".urdf"
)]
else
:
raise
RuntimeError
(
\
"""instance should have one of the following sets of members
- (packageName, urdfNameRom, urdfSuffix),
- (urdfFilenameRom)"""
)
return
nameRoms
,
urdfNameRoms
# # Virtual function to load the robot model.
#
# \param urdfName urdf description of the robot trunk,
# \param urdfNameroms either a string, or an array of strings, indicating the urdf of the different roms to add.
# This method looks for the following class attribute in order to find the files to load:
# First it looks for urdfFilename and srdfFilename and use it if available
# Otherwise it looks for packageName, urdfName, urdfSuffix, srdfSuffix
# \param robotNamethe name of the robot
# \param rootJointType type of root joint among ("freeflyer", "planar",
# "anchor"),
# \param meshPackageName name of the meshpackage from where the robot mesh will be loaded
# \param packageName name of the package from where the robot will be loaded
# \param urdfSuffix optional suffix for the urdf of the robot package
# \param srdfSuffix optional suffix for the srdf of the robot package
def
loadModel
(
self
,
urdfName
,
urdfNameroms
,
rootJointType
,
meshPackageName
,
packageName
,
urdfSuffix
,
srdfSuffix
,
client
=
None
):
Robot
.
__init__
(
self
,
urdfName
,
rootJointType
,
False
,
client
=
client
)
if
(
isinstance
(
urdfNameroms
,
list
)):
for
urdfNamerom
in
urdfNameroms
:
self
.
clientRbprm
.
rbprm
.
loadRobotRomModel
(
urdfNamerom
,
rootJointType
,
packageName
,
urdfNamerom
,
urdfSuffix
,
srdfSuffix
)
else
:
self
.
clientRbprm
.
rbprm
.
loadRobotRomModel
(
urdfNameroms
,
rootJointType
,
packageName
,
urdfNameroms
,
urdfSuffix
,
srdfSuffix
)
# "anchor"), WARNING. Currently RB-PRM only considerds freeflyer roots
def
loadModel
(
self
,
robotName
,
rootJointType
):
nameRoms
,
urdfROMFilenames
=
self
.
urdfROMFilenames
()
for
i
,
urdfNameRom
in
enumerate
(
urdfROMFilenames
):
self
.
clientRbprm
.
rbprm
.
loadRobotRomModel
(
nameRoms
[
i
],
rootJointType
,
urdfNameRom
)
self
.
clientRbprm
.
rbprm
.
initNewProblemSolver
()
self
.
clientRbprm
.
rbprm
.
loadRobotCompleteModel
(
urdfName
,
rootJointType
,
packageName
,
urdfName
,
urdfSuffix
,
srdfSuffix
)
self
.
client
.
robot
.
meshPackageName
=
meshPackageName
self
.
meshPackageName
=
meshPackageName
self
.
packageName
=
packageName
self
.
urdfName
=
urdfName
self
.
urdfSuffix
=
urdfSuffix
self
.
srdfSuffix
=
srdfSuffix
urdfFilename
,
srdfFilename
=
self
.
urdfSrdfFilenames
()
# inherited method from corbaserver.robot
self
.
clientRbprm
.
rbprm
.
loadRobotCompleteModel
(
robotName
,
rootJointType
,
urdfFilename
,
srdfFilename
)
# # Init RbprmShooter
#
...
...
src/hpp/corbaserver/rbprm/rbprmfullbody.py
View file @
2ecd7a4d
...
...
@@ -28,7 +28,8 @@ from numpy import array, matrix
# trunk of the robot, one for the range of motion
class
FullBody
(
Robot
):
# # Constructor
def
__init__
(
self
,
load
=
True
,
clientRbprm
=
None
):
def
__init__
(
self
,
robotName
=
None
,
rootJointType
=
None
,
load
=
True
,
client
=
None
,
hppcorbaClient
=
None
,
clientRbprm
=
None
):
Robot
.
__init__
(
self
,
robotName
,
rootJointType
,
False
,
client
,
hppcorbaClient
)
self
.
tf_root
=
"base_link"
if
clientRbprm
==
None
:
self
.
clientRbprm
=
RbprmClient
()
...
...
@@ -36,27 +37,22 @@ class FullBody(Robot):
self
.
clientRbprm
=
clientRbprm
self
.
load
=
load
self
.
limbNames
=
[]
if
load
:
self
.
loadFullBodyModel
(
robotName
,
rootJointType
)
# # Virtual function to load the fullBody robot model.
#
# \param urdfName urdf description of the fullBody robot
# This method looks for the following class attribute in order to find the files to load:
# First it looks for urdfFilename and srdfFilename and use it if available
# Otherwise it looks for packageName, urdfName, urdfSuffix, srdfSuffix
# \param robotNamethe name of the robot
# \param rootJointType type of root joint among ("freeflyer", "planar",
# "anchor"), WARNING. Currently RB-PRM only considerds freeflyer roots
# \param meshPackageName name of the meshpackage from where the robot mesh will be loaded
# \param packageName name of the package from where the robot will be loaded
# \param urdfSuffix optional suffix for the urdf of the robot package
# \param srdfSuffix optional suffix for the srdf of the robot package
def
loadFullBodyModel
(
self
,
urdfName
,
rootJointType
,
meshPackageName
,
packageName
,
urdfSuffix
,
srdfSuffix
,
client
=
None
):
Robot
.
__init__
(
self
,
urdfName
,
rootJointType
,
False
,
client
)
self
.
clientRbprm
.
rbprm
.
loadFullBodyRobot
(
urdfName
,
rootJointType
,
packageName
,
urdfName
,
urdfSuffix
,
srdfSuffix
,
def
loadFullBodyModel
(
self
,
robotName
,
rootJointType
):
urdfFilename
,
srdfFilename
=
self
.
urdfSrdfFilenames
()
# inherited method from corbaserver.robot
print
(
"selected problem: "
,
self
.
client
.
problem
.
getSelected
(
"problem"
)[
0
])
self
.
clientRbprm
.
rbprm
.
loadFullBodyRobot
(
robotName
,
rootJointType
,
urdfFilename
,
srdfFilename
,
self
.
client
.
problem
.
getSelected
(
"problem"
)[
0
])
self
.
client
.
robot
.
meshPackageName
=
meshPackageName
self
.
meshPackageName
=
meshPackageName
self
.
packageName
=
packageName
self
.
urdfName
=
urdfName
self
.
urdfSuffix
=
urdfSuffix
self
.
srdfSuffix
=
srdfSuffix
self
.
octrees
=
{}
# Virtual function to load the fullBody robot model.
...
...
src/hpp/corbaserver/rbprm/scenarios/abstract_path_planner.py
View file @
2ecd7a4d
from
abc
import
abstractmethod
from
hpp.gepetto
import
ViewerFactory
,
PathPlayer
from
hpp.corbaserver.affordance.affordance
import
AffordanceTool
from
hpp.corbaserver
import
ProblemSolver
from
hpp.corbaserver
import
ProblemSolver
,
Client
from
hpp.corbaserver
import
createContext
,
loadServerPlugin
from
hpp.corbaserver.rbprm
import
Client
as
RbprmClient
class
AbstractPathPlanner
:
...
...
@@ -13,7 +15,11 @@ class AbstractPathPlanner:
extra_dof_bounds
=
None
robot_node_name
=
None
# name of the robot in the node list of the viewer
def
__init__
(
self
):
def
__init__
(
self
,
context
=
None
):
"""
Constructor
:param context: An optional string that give a name to a corba context instance
"""
self
.
v_max
=
-
1
# bounds on the linear velocity for the root, negative values mean unused
self
.
a_max
=
-
1
# bounds on the linear acceleration for the root, negative values mean unused
self
.
root_translation_bounds
=
[
0
]
*
6
# bounds on the root translation position (-x, +x, -y, +y, -z, +z)
...
...
@@ -27,6 +33,17 @@ class AbstractPathPlanner:
self
.
size_foot_y
=
0
# size of the feet along the y axis
self
.
q_init
=
[]
self
.
q_goal
=
[]
self
.
context
=
context
if
context
:
createContext
(
context
)
loadServerPlugin
(
context
,
'rbprm-corba.so'
)
loadServerPlugin
(
context
,
'affordance-corba.so'
)
self
.
hpp_client
=
Client
(
context
=
context
)
self
.
hpp_client
.
problem
.
selectProblem
(
context
)
self
.
rbprm_client
=
RbprmClient
(
context
=
context
)
else
:
self
.
hpp_client
=
None
self
.
rbprm_client
=
None
@
abstractmethod
def
load_rbprm
(
self
):
...
...
@@ -107,6 +124,10 @@ class AbstractPathPlanner:
:param visualize_affordances: list of affordances type to visualize, default to none
"""
vf
=
ViewerFactory
(
self
.
ps
)
if
self
.
context
:
self
.
afftool
=
AffordanceTool
(
context
=
self
.
context
)
self
.
afftool
.
client
.
affordance
.
affordance
.
resetAffordanceConfig
()
# FIXME: this should be called in afftool constructor
else
:
self
.
afftool
=
AffordanceTool
()
self
.
afftool
.
setAffordanceConfig
(
'Support'
,
[
0.5
,
0.03
,
0.00005
])
self
.
afftool
.
loadObstacleModel
(
"package://"
+
env_package
+
"/urdf/"
+
env_name
+
".urdf"
,
...
...
@@ -135,7 +156,7 @@ class AbstractPathPlanner:
else
:
self
.
ps
.
addPathOptimizer
(
"RandomShortcut"
)
def
solve
(
self
):
def
solve
(
self
,
display_roadmap
=
False
):
"""
Solve the path planning problem.
q_init and q_goal must have been defined before calling this method
...
...
@@ -147,6 +168,9 @@ class AbstractPathPlanner:
self
.
ps
.
setInitialConfig
(
self
.
q_init
)
self
.
ps
.
addGoalConfig
(
self
.
q_goal
)
self
.
v
(
self
.
q_init
)
if
display_roadmap
and
self
.
v
.
client
.
gui
.
getNodeList
()
is
not
None
:
t
=
self
.
v
.
solveAndDisplay
(
"roadmap"
,
5
,
0.001
)
else
:
t
=
self
.
ps
.
solve
()
print
(
"Guide planning time : "
,
t
)
...
...
src/hpp/corbaserver/rbprm/scenarios/anymal_contact_generator.py
View file @
2ecd7a4d
...
...
@@ -23,7 +23,7 @@ class AnymalContactGenerator(AbstractContactGenerator):
self
.
fullbody
.
setEndState
(
self
.
q_goal
,
self
.
end_contacts
,
normals_end
)
def
load_fullbody
(
self
):
from
hpp.corbaserver.
rbprm.anymal
import
Robot
from
anymal_
rbprm.anymal
import
Robot
self
.
fullbody
=
Robot
()
self
.
q_ref
=
self
.
fullbody
.
referenceConfig
[::]
+
[
0
]
*
self
.
path_planner
.
extra_dof
self
.
weight_postural
=
self
.
fullbody
.
postureWeights
[::]
+
[
0
]
*
self
.
path_planner
.
extra_dof
...
...
src/hpp/corbaserver/rbprm/scenarios/anymal_path_planner.py
View file @
2ecd7a4d
...
...
@@ -2,8 +2,8 @@ from .abstract_path_planner import AbstractPathPlanner
class
AnymalPathPlanner
(
AbstractPathPlanner
):
def
__init__
(
self
):
super
().
__init__
()
def
__init__
(
self
,
context
=
None
):
super
().
__init__
(
context
)
# set default bounds to a large workspace on x,y with small interval around reference z value
self
.
root_translation_bounds
=
[
-
5.
,
5.
,
-
5.
,
5.
,
0.4
,
0.5
]
# set default used limbs to be both feet
...
...
@@ -19,8 +19,8 @@ class AnymalPathPlanner(AbstractPathPlanner):
self
.
robot_node_name
=
"anymal_trunk"
def
load_rbprm
(
self
):
from
hpp.corbaserver.
rbprm.anymal_abstract
import
Robot
self
.
rbprmBuilder
=
Robot
()
from
anymal_
rbprm.anymal_abstract
import
Robot
self
.
rbprmBuilder
=
Robot
(
client
=
self
.
hpp_client
,
clientRbprm
=
self
.
rbprm_client
)
def
set_joints_bounds
(
self
):
super
().
set_joints_bounds
()
...
...
src/hpp/corbaserver/rbprm/scenarios/demos/talos_flatGround_path.py
View file @
2ecd7a4d
...
...
@@ -2,14 +2,28 @@ from hpp.corbaserver.rbprm.scenarios.talos_path_planner import TalosPathPlanner
class
PathPlanner
(
TalosPathPlanner
):
def
init_problem
(
self
):
self
.
a_max
=
0.1
super
().
init_problem
()
# greatly increase the number of loops of the random shortcut
self
.
ps
.
setParameter
(
"PathOptimization/RandomShortcut/NumberOfLoops"
,
50
)
# force the base orientation to follow the direction of motion along the Z axis
self
.
ps
.
setParameter
(
"Kinodynamic/forceYawOrientation"
,
True
)
def
run
(
self
):
self
.
init_problem
()
self
.
q_init
[:
2
]
=
[
0
,
0
]
self
.
q_goal
[:
2
]
=
[
1
,
0
]
self
.
q_goal
[:
2
]
=
[
1.
,
0
]
# Constraint the initial orientation when forceYawOrientation = True, expressed as a 3D vector (x,y,z)
#self.q_init[-6:-3] = [0.1, 0, 0]
#self.q_goal[-6:-3] = [0, -0.1, 0]
self
.
init_viewer
(
"multicontact/ground"
,
visualize_affordances
=
[
"Support"
])
self
.
init_planner
()
self
.
init_planner
(
True
,
False
)
self
.
solve
()
self
.
display_path
()
# self.play_path()
...
...
src/hpp/corbaserver/rbprm/scenarios/demos/talos_navBauzil_hard_path.py
View file @
2ecd7a4d
...
...
@@ -6,7 +6,7 @@ class PathPlanner(TalosPathPlanner):
from
talos_rbprm.talos_abstract
import
Robot
Robot
.
urdfName
=
"talos_trunk_large"
# load the model with conservative bounding boxes for trunk
self
.
robot_node_name
=
"talos_trunk_large"
self
.
rbprmBuilder
=
Robot
()
self
.
rbprmBuilder
=
Robot
(
client
=
self
.
hpp_client
,
clientRbprm
=
self
.
rbprm_client
)
def
init_problem
(
self
):
super
().
init_problem
()
...
...
src/hpp/corbaserver/rbprm/scenarios/demos/talos_plateformes.py
View file @
2ecd7a4d
...
...
@@ -11,7 +11,7 @@ class ContactGenerator(TalosContactGenerator):
# use a model with upscaled collision geometry for the feet
Robot
.
urdfSuffix
+=
"_safeFeet"
self
.
fullbody
=
Robot
()
self
.
q_ref
=
self
.
fullbody
.
referenceConfig
[::]
+
[
0
]
*
self
.
path_planner
.
extra_dof
self
.
q_ref
=
self
.
fullbody
.
referenceConfig
_legsApart
[::]
+
[
0
]
*
self
.
path_planner
.
extra_dof
self
.
weight_postural
=
self
.
fullbody
.
postureWeights
[::]
+
[
0
]
*
self
.
path_planner
.
extra_dof
def
set_joints_bounds
(
self
):
...
...
src/hpp/corbaserver/rbprm/scenarios/demos/talos_plateformes_path.py
View file @
2ecd7a4d
...
...
@@ -12,8 +12,8 @@ class PathPlanner(TalosPathPlanner):
self
.
root_translation_bounds
=
[
-
5
,
5
,
-
1.5
,
1.5
,
0.95
,
1.3
]
self
.
init_problem
()
self
.
q_init
[:
3
]
=
[
0.1
6
,
0.25
,
1.14
]
self
.
q_goal
[:
3
]
=
[
1.0
9
,
0.25
,
1.14
]
self
.
q_init
[:
3
]
=
[
0.1
8
,
0.25
,
1.14
]
self
.
q_goal
[:
3
]
=
[
1.0
7
,
0.25
,
1.14
]
self
.
init_viewer
(
"multicontact/plateforme_surfaces"
,
reduce_sizes
=
[
0.18
,
0
,
0
],
...
...
src/hpp/corbaserver/rbprm/scenarios/hrp2_contact_generator.py
View file @
2ecd7a4d
...
...
@@ -9,7 +9,7 @@ class HRP2ContactGenerator(AbstractContactGenerator):
self
.
robot_node_name
=
"hrp2_14"
def
load_fullbody
(
self
):
from
h
pp.corbaserver.
rbprm.hrp2
import
Robot
from
h
rp2_
rbprm.hrp2
import
Robot
self
.
fullbody
=
Robot
()
self
.
q_ref
=
self
.
fullbody
.
referenceConfig
[::]
+
[
0
]
*
self
.
path_planner
.
extra_dof
self
.
weight_postural
=
self
.
fullbody
.
postureWeights
[::]
+
[
0
]
*
self
.
path_planner
.
extra_dof
...
...
src/hpp/corbaserver/rbprm/scenarios/hrp2_path_planner.py
View file @
2ecd7a4d
...
...
@@ -2,8 +2,8 @@ from .abstract_path_planner import AbstractPathPlanner
class
HRP2PathPlanner
(
AbstractPathPlanner
):
def
__init__
(
self
):
super
().
__init__
()
def
__init__
(
self
,
context
=
None
):
super
().
__init__
(
context
)
# set default bounds to a large workspace on x,y with small interval around reference z value
self
.
root_translation_bounds
=
[
-
5.
,
5.
,
-
5.
,
5.
,
0.5
,
0.8
]
# set default used limbs to be both feet
...
...
@@ -19,5 +19,5 @@ class HRP2PathPlanner(AbstractPathPlanner):
self
.
robot_node_name
=
"hrp2_trunk_flexible"
def
load_rbprm
(
self
):
from
h
pp.corbaserver.
rbprm.hrp2_abstract
import
Robot
self
.
rbprmBuilder
=
Robot
()
from
h
rp2_
rbprm.hrp2_abstract
import
Robot
self
.
rbprmBuilder
=
Robot
(
client
=
self
.
hpp_client
,
clientRbprm
=
self
.
rbprm_client
)
src/hpp/corbaserver/rbprm/scenarios/hyq_contact_generator.py
View file @
2ecd7a4d
...
...
@@ -23,7 +23,7 @@ class HyqContactGenerator(AbstractContactGenerator):
self
.
fullbody
.
setEndState
(
self
.
q_goal
,
self
.
end_contacts
,
normals_end
)
def
load_fullbody
(
self
):
from
h
pp.corbaserver.
rbprm.hyq
import
Robot
from
h
yq_
rbprm.hyq
import
Robot
self
.
fullbody
=
Robot
()
self
.
q_ref
=
self
.
fullbody
.
referenceConfig
[::]
+
[
0
]
*
self
.
path_planner
.
extra_dof
self
.
weight_postural
=
self
.
fullbody
.
postureWeights
[::]
+
[
0
]
*
self
.
path_planner
.
extra_dof
...
...
src/hpp/corbaserver/rbprm/scenarios/hyq_path_planner.py
View file @
2ecd7a4d
...
...
@@ -2,8 +2,8 @@ from .abstract_path_planner import AbstractPathPlanner
class
HyqPathPlanner
(
AbstractPathPlanner
):
def
__init__
(
self
):
super
().
__init__
()
def
__init__
(
self
,
context
=
None
):
super
().
__init__
(
context
)
# set default bounds to a large workspace on x,y with small interval around reference z value
self
.
root_translation_bounds
=
[
-
5.
,
5.
,
-
5.
,
5.
,
0.5
,
0.75
]
# set default used limbs to be both feet
...
...
@@ -19,8 +19,8 @@ class HyqPathPlanner(AbstractPathPlanner):
self
.
robot_node_name
=
"hyq_trunk_large"
def
load_rbprm
(
self
):
from
h
pp.corbaserver.
rbprm.hyq_abstract
import
Robot
self
.
rbprmBuilder
=
Robot
()
from
h
yq_
rbprm.hyq_abstract
import
Robot
self
.
rbprmBuilder
=
Robot
(
client
=
self
.
hpp_client
,
clientRbprm
=
self
.
rbprm_client
)
def
set_joints_bounds
(
self
):
super
().
set_joints_bounds
()
...
...
src/hpp/corbaserver/rbprm/scenarios/talos_path_planner.py
View file @
2ecd7a4d
...
...
@@ -2,8 +2,8 @@ from .abstract_path_planner import AbstractPathPlanner
class
TalosPathPlanner
(
AbstractPathPlanner
):
def
__init__
(
self
):
super
().
__init__
()
def
__init__
(
self
,
context
=
None
):
super
().
__init__
(
context
)
# set default bounds to a large workspace on x,y with small interval around reference z value
self
.
root_translation_bounds
=
[
-
5.
,
5.
,
-
5.
,
5.
,
0.95
,
1.05
]
# set default used limbs to be both feet
...
...
@@ -20,7 +20,8 @@ class TalosPathPlanner(AbstractPathPlanner):
def
load_rbprm
(
self
):
from
talos_rbprm.talos_abstract
import
Robot
self
.
rbprmBuilder
=
Robot
()
self
.
rbprmBuilder
=
Robot
(
client
=
self
.
hpp_client
,
clientRbprm
=
self
.
rbprm_client
)
self
.
root_translation_bounds
[
-
2
:]
=
[
self
.
rbprmBuilder
.
ref_height
]
*
2
def
set_joints_bounds
(
self
):
super
().
set_joints_bounds
()
...
...
src/hpp/corbaserver/rbprm/tools/constants_and_tools.py
0 → 100644
View file @
2ecd7a4d
import
numpy
as
np
# from hpp.corbaserver.rbprm.hrp2 import Robot as rob
# from hpp.corbaserver.rbprm.tools.obj_to_constraints import load_obj, as_inequalities, rotate_inequalities
# from hpp_centroidal_dynamics import *
# from hpp_spline import *e
from
numpy
import
array
,
hstack
,
identity
,
matrix
,
ones
,
vstack
,
zeros
from
scipy.spatial
import
ConvexHull
# import eigenpy
import
cdd
# from hpp_bezier_com_traj import *
# from qp import solve_lp
Id
=
matrix
([[
1.
,
0.
,
0.
],
[
0.
,
1.
,
0.
],
[
0.
,
0.
,
1.
]])
z
=
array
([
0.
,
0.
,
1.
])
zero3
=
zeros
(
3
)
def
generators
(
A
,
b
,
Aeq
=
None
,
beq
=
None
):
m
=
np
.
hstack
([
b
,
-
A
])
matcdd
=
cdd
.
Matrix
(
m
)
matcdd
.
rep_type
=
cdd
.
RepType
.
INEQUALITY
if
Aeq
is
not
None
:
meq
=
np
.
hstack
([
beq
,
-
Aeq
])
matcdd
.
extend
(
meq
.
tolist
(),
True
)
H
=
cdd
.
Polyhedron
(
matcdd
)
g
=
H
.
get_generators
()
return
[
array
(
g
[
el
][
1
:])
for
el
in
range
(
g
.
row_size
)],
H
def
filter
(
pts
):
hull
=
ConvexHull
(
pts
,
qhull_options
=
'Q12'
)
return
[
pts
[
i
]
for
i
in
hull
.
vertices
.
tolist
()]
def
ineq
(
pts
,
canonicalize
=
False
):
apts
=
array
(
pts
)
m
=
np
.
hstack
([
ones
((
apts
.
shape
[
0
],
1
)),
apts
])
matcdd
=
cdd
.
Matrix
(
m
)
matcdd
.
rep_type
=
cdd
.
RepType
.
GENERATOR
H
=
cdd
.
Polyhedron
(
matcdd
)
bmA
=
H
.
get_inequalities
()
if
canonicalize
:
bmA
.
canonicalize
()
Ares
=
zeros
((
bmA
.
row_size
,
bmA
.
col_size
-
1
))
bres
=
zeros
(
bmA
.
row_size
)
for
i
in
range
(
bmA
.
row_size
):
bmAl
=
array
(
bmA
[
i
])
Ares
[
i
,
:]
=
-
bmAl
[
1
:]
bres
[
i
]
=
bmAl
[
0
]
return
Ares
,
bres
def
ineqQHull
(
hull
):
A
=
hull
.
equations
[:,
:
-
1
]
b
=
-
hull
.
equations
[:,
-
1
]