selfcollisionchecking fails

[bgl91]

hey,
i have a custom robot model (based on the ur5 end extended by an extra joint) for which i would like to plan collisionfree joint trajectories so that the endeffector follows a given path. As i understand the descartes pkg should be superiour to the moveit carthesian planner for such tasks. Since i have a 7 DOF robot and didnt manage to setup a working ikfast ik solver i'm using the trac_ik, which is slower but gives me 50 solutions in 0.5 seconds for a point which is still acceptable as i dont have more than 50-100 points in my carthesian path. I have also disabled the free yaw rotation to boost the planning time.
However in some cases descartes returns and executes (in simulation) a trajectory in which the robot collides with itself without giving any sort of error as seen in this video https://www.youtube.com/watch?v=D99ZIm_JsGM&feature=youtu.be .
All the collision objects are declared in the robot urdf and the collisionchecking is also enabled robot_model_ptr_->setCheckCollisions(true);
Is this a known bug in descartes or did i forget to configure something ?

[yijiangh]

Could you please point out which version (commit) of the Descartes are you using?

[bgl91]

currently i'm using kinetic-devel 429a226

[Jmeyer1292]

I can't advise using Descartes without some kind of IKFast implementation. It's just not meant for it. I think similar ideas can be used with things that implement a numerical interface, but you'd want to tackle the problem differently.

The problem here is that the IK solver did not sample a similar configuration between two subsequent points along the curve you plotted. As a result, it decided to do a joint flip. The start and end of this flip are probably free from collision, but the transition is not.

In MoveIt, they have a parameter (supported by the planners) called longest_valid_segment which checks intermediate joint values for collision. We should support something like that, but I've always wanted to retool how collision checks are done to make it possible.

Anyway, I advise you use MoveIt's computeCartesianPath() function or do your own iterative solving.

We should have better options, but we don't at the moment.

[Jmeyer1292]

This makes me think. I would love to see Descartes grow into a more complete suite of tools for Cartesian path planning. This would include:

• motion planning interfaces for numerical solvers (especially those that can support tolerances through null space optimization, such as CLIK in industrial IK)
• simple (Eigen dependent only) tools for pre-processing trajectories: sampling lines, arcs, and splines at regular intervals
• post-processing trajectories - including sanity/validity checking of trajectories for simple things like limits and collisions, but also maybe some dynamic considerations; Also time parametrization meant to work with time parameterized waypoints.
• Sparse to dense trajectory solving using numerical solvers to do whole trajectory optimization or at least interpolation between solved points. This would include some basic methods (e.g. Taylor's Method) to try and create joint trajectories with "bounded error" from a linear line.
• Search-time, "lazy" constraint checking for collisions, dynamic constraints, etc.

[bgl91]

i have tried to generate an ikfast and a moveit plugin for this robot. However the solution doesnt seem to work with descartes. Even in moveit the interactive marker is hard to drag and apparently can only reach certain positions and orientations. Unfortunately i could only find posts with people having similar problems but no solutions on how to fix ikfast.
Regarding the joint flips they are often induced by the joint limits. I have restricted many of the ur5 joint beyond the -pi/pi because idealy i would like the ur5 itself to do as little movement as possible and let the additional turntable do the movement. However even though i have configured the additional joint to be continuous it seems to be restricted to -pi/pi.

[yijiangh]

@Shadowgolem27 Based on your video, it seems like that your robot model is a robot + end effector configuration. Since ikfast is a "required" component of descartes, I'm wondering if it's necessary for us to have a detailed tutorial for configuring industrial robot with end effector's ik fast component? Given that a more general ikfast tutorial[1] is available in moveit's doc, the proposed tutorial will be more related to ros-i community and its application.

For my own experience, I learned a lot from @JeroenDM 's kuka_kr120_with_torch package [2] in his descartes_tutorial[3] and of course the flagship godel[4] and its abb_irb2400_descartes[5] package, which is, in my opinion, a cleaner solution, seperating end effector package and robot's (please also refer to @gavanderhoorn 's comment on this google group discussion[6]). After experimenting and trying, I build ikfast and descartes interface for kuka kr6 r900[7] for my application.

I'm wondering if such a tutorial would be useful for our community, at least for potential descartes users.

[1] http://docs.ros.org/indigo/api/moveit_ikfast/html/doc/ikfast_tutorial.html
[2] https://github.com/JeroenDM/descartes_tutorials/tree/indigo-devel/kuka_kr120_with_torch
[3] https://github.com/JeroenDM/descartes_tutorials
[4] https://github.com/ros-industrial-consortium/godel
[5] https://github.com/ros-industrial-consortium/godel/tree/kinetic-devel/godel_robots/abb/godel_irb2400/abb_irb2400_descartes
[6] https://groups.google.com/d/msg/swri-ros-pkg-dev/LOzZXvTTgVQ/rXs8PToaBgAJ
[7] https://github.com/yijiangh/framefab_mpp/tree/kinetic-devel/framefab_robot/kuka/framefab_kr6_r900

[BrettHemes]

@Jmeyer1292 regarding your comments

• motion planning interfaces for numerical solvers (especially those that can support tolerances through null space optimization, such as CLIK in industrial IK)
• simple (Eigen dependent only) tools for pre-processing trajectories: sampling lines, arcs, and splines at regular intervals
• post-processing trajectories - including sanity/validity checking of trajectories for simple things like limits and collisions, but also maybe some dynamic considerations; Also time parametrization meant to work with time parameterized waypoints.
• Sparse to dense trajectory solving using numerical solvers to do whole trajectory optimization or at least interpolation between solved points. This would include some basic methods (e.g. Taylor's Method) to try and create joint trajectories with "bounded error" from a linear line.
• Search-time, "lazy" constraint checking for collisions, dynamic constraints, etc.

I agree that these are gaps and would definitely benefit personally from such tools. Do you think they all belong IN Descartes? I could probably justify time in helping to implement some stuff if there is pull...

[yijiangh]

@Jmeyer1292 in your comments

motion planning interfaces for numerical solvers (especially those that can support tolerances through null space optimization, such as CLIK in industrial IK)

I'm wondering if Descartes could benefit from using CLIK to prune IK solutions, adding only feasible solutions with respect to certain constraints instead of adding all IK solutions from ikfast, which could potentially help on solving the memory overhead problem when huge ladder graph needs to be constructed (for example, in my case #200 )?