[ompl-users] ompl_ros_interface for continuous robots

[Meßmer, F. <Fel...@ip...>]

Dear OMPL-users,

I'm currently working on using OMPL MotionPlanning of a continuous-style robot manipulator (SnakeArm).
Since this kind of manipulator does not have rigid-body links connected by revolute joints but rather "flexible" links, I am designing my own approach.

The main idea is to plan a 3D-Cartesian trajectory for the detached SnakeArm tip and then verifying the validity of according SnakeArm configurations for each trajectory point.
For the second step, I implemented my own CollisionChecker using the OcTree structure with distance computation.

However, since I would like to have this planner in ROS, I have some questions about ompl_ros_interface:
(I use Ubuntu 12.04 with ROS Fuerte)

I found that ompl_ros_interface provides interfaces for setting up JointPlanners based on the robot_description and the planning_description.
Especially OmplRos initializes the collision_models_interface_ structure based on robot_description already in its constructor.
Unfortunately, all the planning environment is only suitable for "rigid-body"-like robots.

This is also the reason, why I don't use gazebo for simulation. We use Blender together with Morse.
Also, even if I have a urdf to define the kinematic chain, the links don't have a geometry and the joints are fixed (the actual transformations are published from Blender directly).


Now my questions are:
- Is it possible to use the ompl_ros_interface without using the whole planning_environment structure?
- Is it already possible to setup a 3D Rigid-Body motion planner (similar to the examples coming with OMPL) within ROS? Using the SE§StateSpace?





I already have some draft implementations outside the "ROS-World" based on the OMPL examples where I proofed the integration of my CollisionChecker. (See attached.)
When running the attached file, I get the following error:

fxm@sony-fxm:~/projects/miror/miror/miror_arm_navigation/bin$ ./miror_SnakeArmTCPPlanning 
Reading binary octree type OcTree
read in tree, 80858 leaves 
Compound state [
RealVectorState [11.15 7.1 1.6]
SO3State [0 0 0 0]
]
Compound state [
RealVectorState [9.06 6.5 9.4]
SO3State [0 0 0 0]
]
Start+Goal set
Planner set
Debug:   Planner range detected to be 4.015510
Warning: Skipping invalid start state (invalid bounds)
         at line 249 in /tmp/buildd/ros-fuerte-ompl-0.11.1002045-0precise-20130202-2026/src/ompl/base/src/Planner.cpp
Debug:   Discarded start state Compound state [
RealVectorState [11.15 7.1 1.6]
SO3State [0 0 0 0]
]

Error:   Motion planning start tree could not be initialized!
         at line 173 in /tmp/buildd/ros-fuerte-ompl-0.11.1002045-0precise-20130202-2026/src/ompl/geometric/planners/rrt/src/RRTConnect.cpp
Info:    No solution found after 0.000406 seconds
No solution found


- What is wrong about setting the bounds? It is done the same way in the OMPL example...





If I comment the "set bounds" part, I still don't get a solution:

fxm@sony-fxm:~/projects/miror/miror/miror_arm_navigation/bin$ ./miror_SnakeArmTCPPlanning 
Reading binary octree type OcTree
read in tree, 80858 leaves 
Compound state [
RealVectorState [11.15 7.1 1.6]
SO3State [0 0 0 0]
]
Compound state [
RealVectorState [9.06 6.5 9.4]
SO3State [0 0 0 0]
]
Start+Goal set
Planner set
terminate called after throwing an instance of 'ompl::Exception'
  what():  The longest valid segment for state space RealVectorSpace2 must be positive
Aborted (core dumped)


- What does this message mean? Do I have to adjust some planner settings?




Any help is appreciated!

Best regards,
Felix