From 9304b6ce3329807af2f719be2f66f411174a109b Mon Sep 17 00:00:00 2001
From: Steve Tonneau <stonneau@laas.fr>
Date: Fri, 19 Jun 2015 18:52:09 +0200
Subject: [PATCH] onto path validation rbprm
---
CMakeLists.txt | 26 ++++++++++
data/meshes/box.stl | Bin 684 -> 684 bytes
data/meshes/robot_box.stl | Bin 0 -> 1284 bytes
data/urdf/box.urdf | 4 +-
data/urdf/box_rom.urdf | 4 +-
script/load.py | 20 ++------
script/load2.py | 58 ++++++++++++++++++++++
src/hpp/corbaserver/rbprm/rbprmbuilder.py | 4 +-
src/rbprmbuilder.impl.cc | 15 ++++++
src/rbprmbuilder.impl.hh | 12 +----
10 files changed, 110 insertions(+), 33 deletions(-)
create mode 100644 data/meshes/robot_box.stl
create mode 100644 script/load2.py
diff --git a/CMakeLists.txt b/CMakeLists.txt
index e3b2858..71e55db 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -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()
diff --git a/data/meshes/box.stl b/data/meshes/box.stl
index 37d433d50d312b85d9a2b1e3a02e75964aa01088..f788745d4c2233b819d0b4d6095da0b1645161a6 100644
GIT binary patch
literal 684
zcmb7By=nqM5FRa3rPMlCCPgH~g;eoC2w3C=Y{j#RIk_YtSmb&i!o0(2d2A`7s00%7
z78W*wc$1ku1D|CMc6Z?Wnfc}(`n@oE4ccBO3cFtYDR^!N(e-WRuIgRBytTZWEBe=W
ze!`CJdhvL_lGE9iYFf)lwf>ZMd90lz&V{i>WG;*^Q2#ldDEx0>%;YSagPJf>?W46Q
zQ+3cfch{Tw-A809?4KhGos=8<8J8z#)Ei?CWT84&=zZYCAcD!!NvA1HWFlLn57$Fb
zw;xpFwCX)dWHP(wO%)N`8}@UFZN{O=c#|Lt)wnm8!yb5O_Ixo$1pfy*5vjj!b|)|H
V2h*Zja}14cn5bb8!G7pO^b7fsF`WPa
literal 684
zcmb7By=uZx6h3uu>gsGpJG4l>;3R=Up+ldd7$@6EROn`QAHu#vbiCZL6s0YK(zkFe
zh&i5<tM+qjAonEse$M$$?q7RxGKktiCyAefyKeN-j*{!!n`R@Z^@mSE{fho|U7oR1
z_q}**eaS7=>hq!6rM!RoZ?=%6UYI&W=E7M0sZLI3if}EA56@D@u}@2sRvomjch{47
z6e992=+A+LPRdQgoGY_3^``1^%z-Radxf4m&I}@$9G!HUi<wO1O7vFr5LD;2sK#m4
zdy>gyZp)hnBDgo~S0r{h$0p}ZhAdR$-ijP<$78eQ>J$<DALvA+{<_&;skk3Zi)zg=
PHo9S=2ZIRqLnop=Hl9ce
diff --git a/data/meshes/robot_box.stl b/data/meshes/robot_box.stl
new file mode 100644
index 0000000000000000000000000000000000000000..8e2180da3ee15ddc5e0f5766ea3670d2b8568535
GIT binary patch
literal 1284
zcmb7CyGjE=6dgtV0tKzIr51{q1uL^42o|FF7{peRRBA$05G>p#Rci5#(MGUMwh^*Z
ziYQTph<}m7MhMAx@64{AlrGH7-93-{*qJ-X=L&n7w3995ww>9n%uYH}7@wM$PCBEz
z`<u?>nE2N-+{O0Q<=IH;f)<@2$I%&H(fhgm78;3fg}y@Mlo;hd)vr^R!n+~HiDfY(
z&XXc#$PS#>x<@%YM2-o)jacZ!T;J>HM6fQ4RrZMcKo+X4g7>@o2Nx0S9G!%avOl0a
zs!x<n4nn!!5Y<?%5G%4#Va1YcBoV>6VLnrplCtP(ov0!U)i^h+SS3Y^6o}yeKqu~_
z=<>W4rF3yV*e$AsSdJ8Hq(B7op%YgXKfZc<S(tgHmHiq_aZIbm@jIRMC^l^0YJ7#r
zGcg*Vj-9#`-Ul(BXw^3kQ^cro9*#*_V+W3{JMUT_d5Am``!iyp6La}q4Fv-tU&TI<
zg=(wdy{#)QBG@@PaWy>wRit|9ULgzBSS@>Bn|syL%_Jf?H*_)`tB%gO9o?)T3)MI`
q(}!y7lDSui;Qv4;A=sZM`%Nd#2fIZz_fax8QHXgL5zL29Li_+WO&hoX
literal 0
HcmV?d00001
diff --git a/data/urdf/box.urdf b/data/urdf/box.urdf
index 90b6109..3719aba 100644
--- a/data/urdf/box.urdf
+++ b/data/urdf/box.urdf
@@ -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>
diff --git a/data/urdf/box_rom.urdf b/data/urdf/box_rom.urdf
index a3cce00..bbf27ab 100644
--- a/data/urdf/box_rom.urdf
+++ b/data/urdf/box_rom.urdf
@@ -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>
diff --git a/script/load.py b/script/load.py
index 4fb7bf8..9a68d80 100644
--- a/script/load.py
+++ b/script/load.py
@@ -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 ()
diff --git a/script/load2.py b/script/load2.py
new file mode 100644
index 0000000..4fb7bf8
--- /dev/null
+++ b/script/load2.py
@@ -0,0 +1,58 @@
+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)
+
diff --git a/src/hpp/corbaserver/rbprm/rbprmbuilder.py b/src/hpp/corbaserver/rbprm/rbprmbuilder.py
index e77378d..c43d630 100644
--- a/src/hpp/corbaserver/rbprm/rbprmbuilder.py
+++ b/src/hpp/corbaserver/rbprm/rbprmbuilder.py
@@ -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(urdfNamerom, rootJointType, packageName, urdfName, urdfSuffix, srdfSuffix)
self.name = urdfName
self.displayName = urdfName
self.tf_root = "base_link"
diff --git a/src/rbprmbuilder.impl.cc b/src/rbprmbuilder.impl.cc
index 442641e..734e16c 100644
--- a/src/rbprmbuilder.impl.cc
+++ b/src/rbprmbuilder.impl.cc
@@ -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
diff --git a/src/rbprmbuilder.impl.hh b/src/rbprmbuilder.impl.hh
index 313b7bb..72fa402 100644
--- a/src/rbprmbuilder.impl.hh
+++ b/src/rbprmbuilder.impl.hh
@@ -33,23 +33,15 @@ namespace hpp {
struct BindShooter
{
BindShooter(const std::size_t shootLimit = 10000,
- const std::size_t displacementLimit = 100)
+ 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_;
--
GitLab