diff --git a/CMakeLists.txt b/CMakeLists.txt
index c0b4bb45dd8d0476cc403865f39d34a0bd289e54..e5c7a44023e5b108c17f821e803e14ceeee72148 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -64,19 +64,26 @@ install(FILES
data/urdf/box_rom.urdf
data/urdf/box.urdf
data/urdf/scene.urdf
+ data/urdf/scene2.urdf
DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/urdf
)
install(FILES
data/srdf/box_rom.srdf
data/srdf/box.srdf
data/srdf/scene.srdf
+ data/srdf/scene2.srdf
DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/srdf
)
install(FILES
data/meshes/car.stl
+ data/meshes/car_simple.stl
data/meshes/box.stl
data/meshes/box_rom.stl
data/meshes/robot_box.stl
+ data/meshes/chair_simple.stl
+ data/meshes/ground.stl
+ #~ data/meshes/pedal.stl
+ #~ data/meshes/board.stl
DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/meshes
)
diff --git a/data/car_simple.blend b/data/car_simple.blend
new file mode 100644
index 0000000000000000000000000000000000000000..7923be723b54f0ca1791209344b4b46ca1938043
Binary files /dev/null and b/data/car_simple.blend differ
diff --git a/data/car_simple.blend1 b/data/car_simple.blend1
new file mode 100644
index 0000000000000000000000000000000000000000..fbc8e96cb6263638bc1edeb03b143aca41d5e40e
Binary files /dev/null and b/data/car_simple.blend1 differ
diff --git a/data/meshes/board.stl b/data/meshes/board.stl
new file mode 100644
index 0000000000000000000000000000000000000000..665e7d2a01dfe90dad241c93728092f89566210a
Binary files /dev/null and b/data/meshes/board.stl differ
diff --git a/data/meshes/box.stl b/data/meshes/box.stl
index f788745d4c2233b819d0b4d6095da0b1645161a6..436e2a2c03818997fac9c4e0340dd86aca96136d 100644
Binary files a/data/meshes/box.stl and b/data/meshes/box.stl differ
diff --git a/data/meshes/box_rom.stl b/data/meshes/box_rom.stl
index 4460e9f35bb38801cfd2b4438168f6c49a38816c..9bbb5ec0b7f9c873fea8921885ca642464b29d78 100644
Binary files a/data/meshes/box_rom.stl and b/data/meshes/box_rom.stl differ
diff --git a/data/meshes/car_simple.stl b/data/meshes/car_simple.stl
new file mode 100644
index 0000000000000000000000000000000000000000..65b40c14550c3ee252fe1d92d8c367a7f23b4b9e
Binary files /dev/null and b/data/meshes/car_simple.stl differ
diff --git a/data/meshes/chair_simple.stl b/data/meshes/chair_simple.stl
new file mode 100644
index 0000000000000000000000000000000000000000..f198e43dca453eb2abe5331fc7b6ba2bd843313d
Binary files /dev/null and b/data/meshes/chair_simple.stl differ
diff --git a/data/meshes/ground.stl b/data/meshes/ground.stl
new file mode 100644
index 0000000000000000000000000000000000000000..81373e0ec20193770666f30e1de78063a4806314
Binary files /dev/null and b/data/meshes/ground.stl differ
diff --git a/data/meshes/pedal.stl b/data/meshes/pedal.stl
new file mode 100644
index 0000000000000000000000000000000000000000..ddaa9ebd9869aaf2d67afe1528ca52ad22f53f2d
Binary files /dev/null and b/data/meshes/pedal.stl differ
diff --git a/data/meshes/robot_box.stl b/data/meshes/robot_box.stl
index 8e2180da3ee15ddc5e0f5766ea3670d2b8568535..47c0833c87d3f2457611e8a0c48126ccdd059115 100644
Binary files a/data/meshes/robot_box.stl and b/data/meshes/robot_box.stl differ
diff --git a/data/srdf/scene2.srdf b/data/srdf/scene2.srdf
new file mode 100644
index 0000000000000000000000000000000000000000..1d22c96be1ed3d540631a4b9dbaccc65f7a1330b
--- /dev/null
+++ b/data/srdf/scene2.srdf
@@ -0,0 +1,18 @@
+<?xml version="1.0"?>
+<robot name="scene">
+ <handle name="handle">
+ <position> 0 0 0 1 0 0 0 </position>
+ <link name="base_link"/>
+ </handle>
+ <handle name="handle2">
+ <position> 0 0 0
+ 0 0 0.7071067811865476 0.7071067811865476 </position>
+ <link name="base_link"/>
+ </handle>
+ <contact name="box_surface">
+ <link name="base_link"/>
+ <point>-0.025 -0.025 -0.025 -0.025 0.025 -0.025 -0.025 -0.025 0.025 -0.025 0.025 0.025
+ +0.025 -0.025 -0.025 +0.025 0.025 -0.025 +0.025 -0.025 0.025 +0.025 0.025 0.025 </point>
+ <triangle> 0 2 1 4 5 6</triangle>
+ </contact>
+</robot>
diff --git a/data/urdf/scene.urdf b/data/urdf/scene.urdf
index 02e52d8adbc41cc751f764c42a8fd4d7fa89f582..d0bcbb674c19944c0f2d6c09a50f7be26646b702 100644
--- a/data/urdf/scene.urdf
+++ b/data/urdf/scene.urdf
@@ -3,7 +3,7 @@
<visual>
<origin xyz="0 0 0" rpy="0 0 0" />
<geometry>
- <mesh filename="package:///../hpp-rbprm-corba/meshes/car.stl"/>
+ <mesh filename="package:///../hpp-rbprm-corba/meshes/ground.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>
- <mesh filename="package://hpp-rbprm-corba/meshes/car.stl"/>
+ <mesh filename="package://hpp-rbprm-corba/meshes/ground.stl"/>
</geometry>
</collision>
</link>
diff --git a/data/urdf/scene2.urdf b/data/urdf/scene2.urdf
new file mode 100644
index 0000000000000000000000000000000000000000..6275b4c6e1b0609d04ef18cd372d2d536a559b9e
--- /dev/null
+++ b/data/urdf/scene2.urdf
@@ -0,0 +1,100 @@
+<robot name="scene">
+
+ <!-- This is the top level joint /tf frame. -->
+ <link name="base_link">
+ <!--<sphere_inertia radius="0.001" mass="1"/>-->
+ </link>
+ <joint name="ground_joint" type="fixed">
+ <origin rpy="0 0 0" xyz="0 0 0"/>
+ <parent link="base_link"/>
+ <child link="ground_link"/>
+ </joint>
+ <link name="ground_link">
+ <visual>
+ <origin xyz="0 0 0" rpy="0 0 0" />
+ <geometry>
+ <mesh filename="package:///../hpp-rbprm-corba/meshes/ground.stl"/>
+ </geometry>
+ <material name="white">
+ <color rgba="1 1 1 1"/>
+ </material>
+ </visual>
+ <collision>
+ <origin xyz="0 0 0" rpy="0 0 0" />
+ <geometry>
+ <mesh filename="package://hpp-rbprm-corba/meshes/ground.stl"/>
+ </geometry>
+ </collision>
+ </link>
+
+ <joint name="board_joint" type="fixed">
+ <origin rpy="0 0 0" xyz="0 0 0"/>
+ <parent link="base_link"/>
+ <child link="board_link"/>
+ </joint>
+ <link name="board_link">
+ <visual>
+ <origin xyz="0 0 0" rpy="0 0 0" />
+ <geometry>
+ <mesh filename="package:///../hpp-rbprm-corba/meshes/board.stl"/>
+ </geometry>
+ <material name="white">
+ <color rgba="1 1 1 1"/>
+ </material>
+ </visual>
+ <collision>
+ <origin xyz="0 0 0" rpy="0 0 0" />
+ <geometry>
+ <mesh filename="package://hpp-rbprm-corba/meshes/board.stl"/>
+ </geometry>
+ </collision>
+ </link>
+
+
+ <joint name="chair_joint" type="fixed">
+ <origin rpy="0 0 0" xyz="0 0 0"/>
+ <parent link="base_link"/>
+ <child link="chair_link"/>
+ </joint>
+ <link name="chair_link">
+ <visual>
+ <origin xyz="0 0 0" rpy="0 0 0" />
+ <geometry>
+ <mesh filename="package:///../hpp-rbprm-corba/meshes/chair_simple.stl"/>
+ </geometry>
+ <material name="white">
+ <color rgba="1 1 1 1"/>
+ </material>
+ </visual>
+ <collision>
+ <origin xyz="0 0 0" rpy="0 0 0" />
+ <geometry>
+ <mesh filename="package://hpp-rbprm-corba/meshes/chair_simple.stl"/>
+ </geometry>
+ </collision>
+ </link>
+
+ <joint name="pedal_joint" type="fixed">
+ <origin rpy="0 0 0" xyz="0 0 0"/>
+ <parent link="base_link"/>
+ <child link="pedal_link"/>
+ </joint>
+ <link name="pedal_link">
+ <visual>
+ <origin xyz="0 0 0" rpy="0 0 0" />
+ <geometry>
+ <mesh filename="package:///../hpp-rbprm-corba/meshes/pedal.stl"/>
+ </geometry>
+ <material name="white">
+ <color rgba="1 1 1 1"/>
+ </material>
+ </visual>
+ <collision>
+ <origin xyz="0 0 0" rpy="0 0 0" />
+ <geometry>
+ <mesh filename="package://hpp-rbprm-corba/meshes/pedal.stl"/>
+ </geometry>
+ </collision>
+ </link>
+
+</robot>
diff --git a/script/load.py b/script/load.py
index 9a68d80bfbe49263935b7624daa21b97b7bcf15c..a9243414fd522fdb3fab0e40a24d2941f9992224 100644
--- a/script/load.py
+++ b/script/load.py
@@ -12,7 +12,7 @@ srdfSuffix = ""
rbprmBuilder = Builder ()
rbprmBuilder.loadModel(urdfName, urdfNameRom, rootJointType, meshPackageName, packageName, urdfSuffix, srdfSuffix)
-rbprmBuilder.setJointBounds ("base_joint_xyz", [0, 2, -2, 0, 0, 1.5])
+rbprmBuilder.setJointBounds ("base_joint_xyz", [0, 2, -2, 0, -1, 1.5])
#~ from hpp.corbaserver.rbprm. import ProblemSolver
from hpp.corbaserver.rbprm.problem_solver import ProblemSolver
@@ -23,22 +23,22 @@ r = Viewer (ps)
q_init = rbprmBuilder.getCurrentConfig ()
q_goal = q_init [::]
-q_init [0:3] = [0, -0.5, 0]
-q_goal [0:3] = [1.1, -0.5, 1]
+q_init [0:3] = [0, -0.5, 0.3]
+q_goal = [0, -0.5, -0.2, -0.501544,0.431183, 0.662926, -0.350804]
r (q_goal)
-r.loadObstacleModel (packageName, "scene", "car")
ps.setInitialConfig (q_init)
ps.addGoalConfig (q_goal)
ps.client.problem.selectConFigurationShooter("RbprmShooter")
ps.client.problem.selectPathValidation("RbprmPathValidation",0.05)
+r.loadObstacleModel (packageName, "scene", "car")
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 c43d630bca6f09ea162b5638cf327c6fbb7aae9c..9a47066e71f9d20665b2cc706b95aa63a4e7409a 100644
--- a/src/hpp/corbaserver/rbprm/rbprmbuilder.py
+++ b/src/hpp/corbaserver/rbprm/rbprmbuilder.py
@@ -50,8 +50,8 @@ class Builder (object):
## Virtual function to load the robot model
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(urdfNamerom, rootJointType, packageName, urdfName, urdfSuffix, srdfSuffix)
+ self.client.rbprm.rbprm.loadRobotRomModel(urdfNamerom, rootJointType, packageName, urdfNamerom, urdfSuffix, srdfSuffix)
+ self.client.rbprm.rbprm.loadRobotCompleteModel(urdfName, rootJointType, packageName, urdfName, urdfSuffix, srdfSuffix)
self.name = urdfName
self.displayName = urdfName
self.tf_root = "base_link"
diff --git a/src/rbprmbuilder.impl.hh b/src/rbprmbuilder.impl.hh
index 19e2131e51238d903c523d98eafb9cd8897fdd39..eedf7d1015ff34d453e58c4ac430312e3b3523f4 100644
--- a/src/rbprmbuilder.impl.hh
+++ b/src/rbprmbuilder.impl.hh
@@ -32,7 +32,7 @@ namespace hpp {
struct BindShooter
{
BindShooter(const std::size_t shootLimit = 10000,
- const std::size_t displacementLimit = 10)
+ const std::size_t displacementLimit = 1000)
: shootLimit_(shootLimit)
, displacementLimit_(displacementLimit) {}