Commit d7acbd31 authored by Steve Tonneau's avatar Steve Tonneau
Browse files

rbprm now working m...

parent 1843b3a2
......@@ -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
)
......
No preview for this file type
No preview for this file type
<?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>
......@@ -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>
......
<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>
......@@ -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)
......@@ -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"
......
......@@ -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) {}
......
Supports Markdown
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment