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
9304b6ce
Commit
9304b6ce
authored
Jun 19, 2015
by
Steve Tonneau
Browse files
onto path validation rbprm
parent
be06e21f
Changes
10
Hide whitespace changes
Inline
Side-by-side
CMakeLists.txt
View file @
9304b6ce
...
...
@@ -53,5 +53,31 @@ ADD_SUBDIRECTORY(src)
CONFIG_FILES
(
include/hpp/corbaserver/rbprm/doc.hh
)
SET
(
CATKIN_PACKAGE_SHARE_DESTINATION
${
CMAKE_INSTALL_DATAROOTDIR
}
/
${
PROJECT_NAME
}
)
install
(
FILES
data/package.xml
DESTINATION
${
CATKIN_PACKAGE_SHARE_DESTINATION
}
)
install
(
FILES
data/urdf/box_rom.urdf
data/urdf/box.urdf
data/urdf/scene.urdf
DESTINATION
${
CATKIN_PACKAGE_SHARE_DESTINATION
}
/urdf
)
install
(
FILES
data/srdf/box_rom.srdf
data/srdf/box.srdf
data/srdf/scene.srdf
DESTINATION
${
CATKIN_PACKAGE_SHARE_DESTINATION
}
/srdf
)
install
(
FILES
data/meshes/car.stl
data/meshes/box.stl
data/meshes/box_rom.stl
DESTINATION
${
CATKIN_PACKAGE_SHARE_DESTINATION
}
/meshes
)
SETUP_PROJECT_FINALIZE
()
data/meshes/box.stl
View file @
9304b6ce
No preview for this file type
data/meshes/robot_box.stl
0 → 100644
View file @
9304b6ce
File added
data/urdf/box.urdf
View file @
9304b6ce
...
...
@@ -3,7 +3,7 @@
<visual>
<origin xyz="0 0 0" rpy="0 0 0" />
<geometry>
<
box size="0.05 0.05 0.05
"/>
<
mesh filename="package://hpp-rbprm-corba/meshes/robot_box.stl
"/>
</geometry>
<material name="white">
<color rgba="1 1 1 1"/>
...
...
@@ -12,7 +12,7 @@
<collision>
<origin xyz="0 0 0" rpy="0 0 0" />
<geometry>
<
box size="0.05 0.05 0.05
"/>
<
mesh filename="package://hpp-rbprm-corba/meshes/box.stl
"/>
</geometry>
</collision>
</link>
...
...
data/urdf/box_rom.urdf
View file @
9304b6ce
...
...
@@ -3,7 +3,7 @@
<visual>
<origin xyz="0 0 0" rpy="0 0 0" />
<geometry>
<
box size="0.1 0.1 0.1
"/>
<
mesh filename="package://hpp-rbprm-corba/meshes/box_rom.stl
"/>
</geometry>
<material name="white">
<color rgba="1 1 1 1"/>
...
...
@@ -12,7 +12,7 @@
<collision>
<origin xyz="0 0 0" rpy="0 0 0" />
<geometry>
<
box size="0.1 0.1 0.1
"/>
<
mesh filename="package://hpp-rbprm-corba/meshes/box_rom.stl
"/>
</geometry>
</collision>
</link>
...
...
script/load.py
View file @
9304b6ce
...
...
@@ -23,22 +23,8 @@ r = Viewer (ps)
q_init
=
rbprmBuilder
.
getCurrentConfig
()
q_goal
=
q_init
[::]
q_init
[
0
:
3
]
=
[
0
,
-
0.5
,
0.4
]
#~ rank = rbprmBuilder.rankInConfiguration ['torso_lift_joint']
#~ q_init [rank] = 0.2
r
(
q_init
)
q_goal
[
0
:
3
]
=
[
1
,
-
0.5
,
1
]
#~ q_goal [0:3] = [-3.2, 0, 3]
#~ rank = rbprmBuilder.rankInConfiguration ['l_shoulder_lift_joint']
#~ q_goal [rank] = 0.5
#~ rank = rbprmBuilder.rankInConfiguration ['l_elbow_flex_joint']
#~ q_goal [rank] = -0.5
#~ rank = rbprmBuilder.rankInConfiguration ['r_shoulder_lift_joint']
#~ q_goal [rank] = 0.5
#~ rank = rbprmBuilder.rankInConfiguration ['r_elbow_flex_joint']
#~ q_goal [rank] = -0.5
q_init
[
0
:
3
]
=
[
0
,
-
0.5
,
0
]
q_goal
[
0
:
3
]
=
[
1.1
,
-
0.5
,
1
]
r
(
q_goal
)
r
.
loadObstacleModel
(
packageName
,
"scene"
,
"car"
)
...
...
@@ -47,7 +33,7 @@ ps.setInitialConfig (q_init)
ps
.
addGoalConfig
(
q_goal
)
ps
.
client
.
problem
.
selectConFigurationShooter
(
"RbprmShooter"
)
ps
.
client
.
problem
.
selectPathValidation
(
"RbprmPathValidation"
,
0.05
)
ps
.
solve
()
...
...
script/load2.py
0 → 100644
View file @
9304b6ce
from
hpp.corbaserver.rbprm.rbprmbuilder
import
Builder
from
hpp.gepetto
import
Viewer
rootJointType
=
'freeflyer'
packageName
=
'hpp-rbprm-corba'
meshPackageName
=
'hpp-rbprm-corba'
urdfName
=
'box'
urdfNameRom
=
'box_rom'
urdfSuffix
=
""
srdfSuffix
=
""
rbprmBuilder
=
Builder
()
rbprmBuilder
.
loadModel
(
urdfName
,
urdfNameRom
,
rootJointType
,
meshPackageName
,
packageName
,
urdfSuffix
,
srdfSuffix
)
rbprmBuilder
.
setJointBounds
(
"base_joint_xyz"
,
[
0
,
2
,
-
2
,
0
,
0
,
1.5
])
#~ from hpp.corbaserver.rbprm. import ProblemSolver
from
hpp.corbaserver.rbprm.problem_solver
import
ProblemSolver
ps
=
ProblemSolver
(
rbprmBuilder
)
r
=
Viewer
(
ps
)
q_init
=
rbprmBuilder
.
getCurrentConfig
()
q_goal
=
q_init
[::]
q_init
[
0
:
3
]
=
[
0
,
-
0.5
,
0.4
]
#~ rank = rbprmBuilder.rankInConfiguration ['torso_lift_joint']
#~ q_init [rank] = 0.2
r
(
q_init
)
q_goal
[
0
:
3
]
=
[
1
,
-
0.5
,
1
]
#~ q_goal [0:3] = [-3.2, 0, 3]
#~ rank = rbprmBuilder.rankInConfiguration ['l_shoulder_lift_joint']
#~ q_goal [rank] = 0.5
#~ rank = rbprmBuilder.rankInConfiguration ['l_elbow_flex_joint']
#~ q_goal [rank] = -0.5
#~ rank = rbprmBuilder.rankInConfiguration ['r_shoulder_lift_joint']
#~ q_goal [rank] = 0.5
#~ rank = rbprmBuilder.rankInConfiguration ['r_elbow_flex_joint']
#~ q_goal [rank] = -0.5
r
(
q_goal
)
r
.
loadObstacleModel
(
packageName
,
"scene"
,
"car"
)
ps
.
setInitialConfig
(
q_init
)
ps
.
addGoalConfig
(
q_goal
)
ps
.
client
.
problem
.
selectConFigurationShooter
(
"RbprmShooter"
)
ps
.
solve
()
from
hpp.gepetto
import
PathPlayer
pp
=
PathPlayer
(
rbprmBuilder
.
client
.
basic
,
r
)
pp
(
0
)
src/hpp/corbaserver/rbprm/rbprmbuilder.py
View file @
9304b6ce
...
...
@@ -49,9 +49,9 @@ class Builder (object):
self
.
load
=
load
## Virtual function to load the robot model
def
loadModel
(
self
,
urdfName
,
rootJointType
,
packageName
,
meshPackageName
,
urdfSuffix
,
srdfSuffix
):
def
loadModel
(
self
,
urdfName
,
urdfNamerom
,
rootJointType
,
packageName
,
meshPackageName
,
urdfSuffix
,
srdfSuffix
):
self
.
client
.
rbprm
.
rbprm
.
loadRobotRomModel
(
urdfName
,
rootJointType
,
packageName
,
urdfName
,
urdfSuffix
,
srdfSuffix
)
self
.
client
.
rbprm
.
rbprm
.
loadRobotCompleteModel
(
urdfName
,
rootJointType
,
packageName
,
urdfName
,
urdfSuffix
,
srdfSuffix
)
self
.
client
.
rbprm
.
rbprm
.
loadRobotCompleteModel
(
urdfName
rom
,
rootJointType
,
packageName
,
urdfName
,
urdfSuffix
,
srdfSuffix
)
self
.
name
=
urdfName
self
.
displayName
=
urdfName
self
.
tf_root
=
"base_link"
...
...
src/rbprmbuilder.impl.cc
View file @
9304b6ce
...
...
@@ -19,8 +19,11 @@
#include
<hpp/util/debug.hh>
#include
"rbprmbuilder.impl.hh"
#include
"hpp/rbprm/rbprm-device.hh"
#include
"hpp/rbprm/rbprm-validation.hh"
#include
"hpp/model/urdf/util.hh"
#include
<hpp/core/collision-path-validation-report.hh>
#include
<hpp/core/problem-solver.hh>
#include
<hpp/core/discretized-collision-checking.hh>
...
...
@@ -95,6 +98,17 @@ namespace hpp {
}
}
namespace
{
hpp
::
core
::
PathValidationPtr_t
createPathValidation
(
const
hpp
::
model
::
DevicePtr_t
&
robot
,
const
hpp
::
model
::
value_type
&
val
)
{
hpp
::
model
::
RbPrmDevicePtr_t
robotcast
=
boost
::
static_pointer_cast
<
hpp
::
model
::
RbPrmDevice
>
(
robot
);
hpp
::
rbprm
::
RbPrmValidationPtr_t
validation
(
hpp
::
rbprm
::
RbPrmValidation
::
create
(
robotcast
));
hpp
::
core
::
CollisionPathValidationReport
defaultValidation
;
return
hpp
::
core
::
DiscretizedCollisionChecking
::
createWithValidation
(
robot
,
val
,
defaultValidation
,
validation
);
}
}
void
RbprmBuilder
::
SetProblemSolver
(
hpp
::
core
::
ProblemSolverPtr_t
problemSolver
)
{
problemSolver_
=
problemSolver
;
...
...
@@ -104,6 +118,7 @@ namespace hpp {
// add rbprmshooter
problemSolver
->
addConfigurationShooterType
(
"RbprmShooter"
,
boost
::
bind
(
&
BindShooter
::
create
,
boost
::
ref
(
bindShooter_
),
_1
));
problemSolver
->
addPathValidationType
(
"RbprmPathValidation"
,
&
createPathValidation
);
}
}
// namespace impl
...
...
src/rbprmbuilder.impl.hh
View file @
9304b6ce
...
...
@@ -33,23 +33,15 @@ namespace hpp {
struct
BindShooter
{
BindShooter
(
const
std
::
size_t
shootLimit
=
10000
,
const
std
::
size_t
displacementLimit
=
10
0
)
const
std
::
size_t
displacementLimit
=
10
)
:
shootLimit_
(
shootLimit
)
,
displacementLimit_
(
displacementLimit
)
{}
hpp
::
rbprm
::
RbPrmShooterPtr_t
create
(
const
hpp
::
model
::
DevicePtr_t
&
robot
)
{
hpp
::
model
::
DevicePtr_t
pRobot
=
robot
;
hpp
::
model
::
RbPrmDevicePtr_t
robotcast
=
boost
::
static_pointer_cast
<
hpp
::
model
::
RbPrmDevice
>
(
robot
);
hpp
::
rbprm
::
RbPrmValidationPtr_t
validator
=
hpp
::
rbprm
::
RbPrmValidation
::
create
(
robotcast
);
const
hpp
::
core
::
ObjectVector_t
&
obstacles
=
problemSolver_
->
collisionObstacles
();
for
(
hpp
::
core
::
ObjectVector_t
::
const_iterator
cit
=
obstacles
.
begin
();
cit
!=
obstacles
.
end
();
++
cit
)
{
validator
->
addObstacle
(
*
cit
);
}
return
hpp
::
rbprm
::
RbPrmShooter
::
create
(
robotcast
,
problemSolver_
->
collisionObstacles
(),
validator
,
shootLimit_
,
displacementLimit_
);
(
robotcast
,
problemSolver_
->
problem
()
->
collisionObstacles
(),
shootLimit_
,
displacementLimit_
);
}
hpp
::
core
::
ProblemSolverPtr_t
problemSolver_
;
std
::
size_t
shootLimit_
;
...
...
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