Moveit: GSoC: Motion planning with general end-effector constraints in MoveIt

[JeroenDM]

mentors: @mamoll @ommmid @felixvd

This summer I would like to add collision-free motion planning with general end-effector constraints to MoveIt. Application examples include robot arc welding, grinding, and painting. The original discussion on discourse can be found here. I probed for interest in other issues here and here. The goal of this issue is to log and discuss my progress in the coming months.

Initial project description

Many industrial robot applications specify a Cartesian end-effector path, that could be under-constrained. The MoveIt’s RobotState function, “computeCartesianPath”, fulfills only a small part of this demand. OMPL has advanced capabilities to handle constraints that are not integrated with MoveIt yet. Examples of applications that can benefit from this integration are robot welding, grinding, painting, and many more.

Concretely this would mean integrating OMPL’s Cartesian planning capabilities in the existing planning plugin. If time allows it, I would like to add support for Cartesian path constraints in TrajOpt. All deliverables should include documentation, tutorials, and tests.

Planning

Phase 1: Example cases and interface discussion (June)

• Find/create example cases that use external planners to solve the planning problem and showcase what will be possible with MoveIt at the end of the project. An example could be the puzzle piece demo from trajot_ros or a welding example such as the last one in hybrid_planning_examples.
• The current interface to specify path constraints in Moveit is not entirely clear, I suspect it will take some time to find a common interface to specify these planning problems.
• As a learning experience, I could refactor the current “computeCartesianPath” as a MoveIt planning plugin, as discussed here.

Phase 2: Implementation (July)

• Start implementing the features that we agreed upon in the first month. This will most likely mean working in the code of the current OMPL interface. I don’t know the exact approach yet, as explained below in the comments from the kick-off meeting.
• @v4hn also pointed out that the broken pose_model_state_space should be replaced. (I did not look into the details of this yet.)

Phase 3: Documentation and TrajOpt Cartesian constraints (August)

• Finish the implementation and tests.
• I would really like to spend some time writing a good tutorial on how to use the new features.
• Add the missing cost terms for the Cartesian constraint in TrajOpt, as discussed here.

In addition, it would be nice to have some benchmarks comparing, for example, OMPL with TrajOpt.

Interface

A large unresolved point is how to model the constraints and describe them in MoveIt. Planners such as Descartes use discrete waypoints along an end-effector path. There are several options to integrate this type of constraints:

• moveit_msgs/MotionPlanRequest.msg the trajectory_constraints field, possible renamed to waypoints as discussed here.
• moveit_msgs/MotionSequenceRequest.
• MoveGroupCartesianPathService as done for Descartes here.

On the other hand, the constrained planners in OMPL require a generic constraint function F(q) = 0 (and ideally the Jacobian). For simple cases, this could be put into the path_constraints of a motion planning request. For more general cases I have no (good) idea yet of how to do this. Possible, the concept of Task Space Regions can be helpful here.

In addition, this Contact Dynamic Roadmaps (CDRM) planner, used for quadruped locomotion and welding problems, could provide inspiration. In the interface, you cannot only add constraints, but also a desired nominal value that can be used in a cost function for optimization.

Links

• Proposal
• GSoC log
• Original discussion of the proposal.
• Playlist with short videos I refer to in the comments below.

[JeroenDM]

Comments during the kick-off meeting

@mamoll mentioned an alternative to full integration of OMPL’s ConstraintStateSpace is to implement something similar in MoveIt by extending the JointModelStateSpace and using the existing interface to plan within this state space.

This would mean reimplementing the techniques to handle Constraints currently available in OMPL, ProjectedStateSpace, AtlasStateSpace, and TangentBundleStateSpace. Only the first one seems feasible to me within the given timeline. An advantage of reimplementing is that it would be easy to add explicit sampling of the constraints as done in Descartes.

@ommmid explained that supporting Cartesian path constraints in TrajOpt should not be too difficult. But currently, the collision cost in TrajOpt is not finished yet. This would be beyond the scope of my project.

[v4hn]

by extending the JointModelStateSpace and using the existing interface to plan within this state space.

@mamoll sure knows what he's talking about and has his reasons to propose this.
Having spent several weeks of developement time understanding internals of ModelBasedPlanningContext and its derivatives in past years,
I would greatly appreciate not adding more complexity there if it can be avoided.
The PoseModelStateSpace was an attempt for eef planning implemented in such a wrapper and it's a horrible broken mess...

[felixvd]

So, is that an argument against the proposed implementation...? I'm not familiar with that part of the code.

[v4hn]

So, is that an argument against the proposed implementation...? I'm not familiar with that part of the code.

Rewriting the ompl plugin ModelBasedStateSpace entirely would definitely be an option,
but I think it does not make sense to build on that code even more structure.

Nor am I familiar with the OMPL codebase for handling manifolds. Maybe it's even worse. :D
Otherwise it would probably be cleaner and include more functionality to wrap OMPLs support here.

[JeroenDM]

by extending the JointModelStateSpace and using the existing interface to plan within this state space.

@mamoll sure knows what he's talking about and has his reasons to propose this.

I think it was more mentioned as an alternative approach because it had been done like this in the past in a closed source project. I don't think we discussed in great detail why this would be a better approach.

Otherwise it would probably be cleaner and include more functionality to wrap OMPLs support here.

I certainly agree that it would be better if we could reuse as much code as possible that already exists in OMPL.

[JeroenDM]

I did not look into the details of the MoveIt code yet but went through the constrained planning documentation on ompl's website. In this process I revamped some old code I had lying around that implements an SE3 state space that inherits from RealVectorStateSpace and therefore can be used by the constrained planning algorithms. Although I do not expect to do something along these lines here since I will be doing planning for robot arms parameterized in joint space.

I'm also happy to see Determinism and Fully featured Cartesian Planner on the MoveIt roadmap. Deterministic state samplers where added to ompl recently.

Unfortunately, I had quite a lot of other work this week that prevented me from a more elaborate preparation of my GSoC project.

[JeroenDM]

I went through all of the ompl interface code and created a diagram to get a general idea of what's in it: moveit_ompl_interface_diagram.pdf.

Some general questions I asked myself when reading the code:

• Why are two classes (OMPLPlannerManager and ConstraintApproximationStateSampler) defined in a source file instead of the header file? (@ommmid maybe you know this, as it is also the case in the TrajOpt plugin?)
• The sbpl planner inherits from planning_interface::Planner instead of planning_interface::PlannerManager. Is this an artifact from an older version of the planning interface?
• Why not inherit from ompl's RealVectorStateSpace instead of StateSpace? This would allow for some reuse of sampling methods implemented in OMPL.
• JointModelStateSpace does not do much compared to it's parent class ModelBasedStateSpace. It is probably there to differentiate it from PoseModelStateSpace, which does specialize.
• It will be helpful if I figure out in more detail which parts of the code are actually used at the moment.

I'll dive into it on World MoveIt day tomorrow to find answers and even better questions.

[ommmid]

In the same context, I had looked at the OMPL inteface in moveit too and here is the graph of it just as a reference and giving more information.

About why OMPLPLannerManager does not have any header:
CHOMPPlannerManager class also is declared and defined in the source file. OMPL interface did the same thing. I just followed the structure for trajopt as well. I am not sure why that is but maybe because it does not need to be shared with any other files. Thus, it does not need to have a header file, making it easier for the compiler?

My understanding is:
OMPL/CHOMP/TrajOptPlannerManager are mostly only used to override the virtual functions and that is it, no new method is created in these derived classs. So why bother to create any header file for them? they are not going to be shared with anything else. Including the base calss which is PlannerManager (from planning_interface.h) should be enough.
while if you look at ModelBasedPlanningContext, it has bunch of other methods that are new, specific to this derived class and not overriding anything. So we need to share its header if we want to used this new methods.

[JeroenDM]

@ommmid Thanks for the graph. It's helpful, as was the tutorial you wrote on MoveIt plugins. I went through it today and proposed some minor improvements in a pull request.

no new method is created in these derived classs. So why bother to create any header file for them?

That makes sense. I was used to reading header files as documentation, hence my confusion :)

My plan of action now is to start with refactor the current “computeCartesianPath” as a MoveIt planning plugin as mentioned in the top post. This should be a good opportunity to propose an interface and learn the details of planning plugins.

[JeroenDM]

I uploaded an example of the type of problem I would like to start with here.
(I will first have to clean up the code I used to generate this video, as this is currently part of my bulky attempt at benchmarking complex planning problems in this repository).

I will first implement an example that does Cartesian planning using the reference_trajectories field in a MotionPlanRequest, but does not yet use OMPL for Cartesian planning.

As far as I know there is currently no planner that supports a Cartesian trajectory in reference_trajectories. There is an example for a joint trajectory in the TrajOpt example code. A would use some very basic proof of concept, or maybe Descartes, for this first version.

[ommmid]

I uploaded an example of the type of problem I would like to start with here.

Looks like a good problem for Cartesian planning.

As far as I know there is currently no planner that supports a Cartesian trajectory in reference_trajectories. There is an example for a joint trajectory in the TrajOpt example code. A would use some very basic proof of concept, or maybe Descartes, for this first version.

reference_trajectories was added quite recently for optimization based planners. So yes, other planners have not used this. In TrajOpt it is used as an initial guess trajectory.

[mamoll]

@JeroenDM I like the welding problem in the YouTube video you linked to. If it is easy to test out whether TrajOpt can solve this problem (while ignoring collisions for now), then give that a try.

For OMPL it's not that easy to incorporate a reference trajectory. The internal data structure that each planner in OMPL is different, so they each need to do something different with the additional info provided by a reference trajectory. One approach that I have seen used is to treat an initial trajectory as a sequence of _n_ states. This is fed to a special sampler that first generates the _n_ states and then falls back on the default random sampling. This might help speed up OMPL, but I would not worry about this initially. Just getting constrained _global_ planning working without this bootstrapping optimization is hard enough.

reference_trajectories was added quite recently for optimization based planners. So yes, other planners have not used this. In TrajOpt it is used as an initial guess trajectory.

@ommmid wasn't the idea that another planning pipeline could seed a trajectory optimization method with a _feasible_ initial guess?

[ommmid]

@ommmid wasn't the idea that another planning pipeline could seed a trajectory optimization method with a _feasible_ initial guess?

I am not aware of that but it seems a better idea for feasible initial guess.

[JeroenDM]

@mamoll

If it is easy to test out whether TrajOpt can solve this problem

As soon as the TrajOpt plugin integrates collision checking it should be easy to do, (by adding the Cartesian cost terms). I could also use the tesseract version of TrajOpt to do in now of course.

For OMPL it's not that easy to incorporate a reference trajectory.

That's an intersting idea I did not think of (oddly enough). I planned to use the reference trajectory as nominal values to center the constraints around. It is not trivial to convert these constraints to global constraints (F(x) = 0) for OMPL, but I had some ideas.

Anyway, I will continue with my prove of concept tomorrow and provide a more detailed proposal.

[JeroenDM]

Given the welding example posted earlier, I decided to leave out the free space motion in between the welds. This allows me to focus on the constrained planning part.
When constrained planning works properly, the MoveIt Task Constructor can be used to combine the different sub problems.

As a starting point I tried solving the welding case using descartes_capabilities, but I there are some still some issues, especially when adding the work object (not shown below).
I will clean up and push my code to a public repository tomorrow before I continue working on it.

[JeroenDM]

I created a repository with some examples. I experimented a bit with the different formats to specify the constrained planning problems and wrote down my conclusion in doc/interface.md. The main difficulty, how to represent the constraints for ompl, is discussed in the second section.

Now I plan to dive into the details of how the ompl interface currently represents and samples the constraints and identify the specific parts I have to change/extend.

I did not create a planning plugin for the basic Cartesian planner, as alluded to earlier. I think I understand the basic plugin interface enough for now. And since the code is already taken out of the robot state class there is maybe not so much value in adding this.

[JeroenDM]

I noticed I forgot to post my work of the end of last week here.

I started working on a minimal example using OMPL's ProjectedStateSpace to do constrained planning in this repository.

(I keep a daily log of my work here as mentioned in the first comment.)

[JeroenDM]

Finally, the visualization is working so I could make a video of the demo mentioned above :)
https://youtu.be/KvgvV8T2zvs

The x-position equality constraint uses the Jacobian from MoveIt, the bounds on the z-position not yet.

[JeroenDM]

I wrote down what I'm trying out in code more formally, so it is easier to check if it makes sense.
constraints_model.pdf

I'm having trouble getting the Orientation constraints to work. I plan to create a simpler example with a 6 dof robot for debugging. (I was using the panda robot now.)
Edit: a specific example of a point-to-point motion, where the end-effector pitch angle in constrained between pi/2 +/- 0.5. Sometimes planning works and I get a straight forward path from A to B.

More often, I get a funky path. OMPL seems to add goal states that are far away from the original goal, and finds an approximate solution that looks like this:

The implementation of TrajOpt by ROS-Industrial seemed to have similar issues. They used finite differences for a specific parts of the Jacobian to solve the issue, but for angle-axis error instead of roll pitch yaw angles.

I compared the analytical Jacobian with a finite differences one for RPY angles, and it seems to work. But I'm not sure if I'm using it correctly when using it for the Jacobian of the constraints here. Could be that I'm doing something wrong related to things being expressed in different reference frames.

[JeroenDM]

Meeting notes 15/06/2020

@felixvd @mamoll @ommmid

Orientation constraints
In the previous comment, I illustrated an issue I encountered with orientation constraints. These constraints are specified using roll, pitch, and yaw angles, as implemented in MoveIt's constraint samplers. (Also see this new issue.) This is intuitive for the user, but probably causes convergence problems for the constraint projection methods. Using unit quaternions should give better results, but it is more difficult for users to specify orientation constraints. I think I will continue working as follows:

1. Start with a fixed end-effector pose as orientation constraints. This can be specified and implemented using quaternions.
2. Try using angle-axis tolerance specification as used by TrajOpt.

An anternative more esoteric solution Mark proposed is to formulate constraints as a region on the 4D unit sphere (which is the space quaternions live in). A 3D Ellipsoid could represent something useful maybe? (Or a Squircle?). Something in this direction already exists in OMPL in SO3StateSampler::sampleUniformNear for correct (unbiased) uniform sampling of SO3. (Written by @mamoll ?)

Jacobians
It was not entirely clear how I used the Jacobian here as explained in constrained_model.pdf. Maybe to clarify this a bit it is useful to mention how the Jacobian will be used by the constrained planner inside OMPL. This is explained on page 15 of the paper Sampling-Based Methods for Motion Planning with Constraints

General remarks

• Omid: Add instructions to run the example code I write. TODO
• Felix: In example videos, visualize start state, goal state, and path constraints. TODO
• Everyone: Reach out during the week if I’m stuck on a problem.
• I will take some vacation time next week Monday and Tuesday, if the weather is nice at least :)

I hope I did not miss anything mentioned in the meeting.

[JeroenDM]

I created another very simple planning case for debugging, using a 6 DOF Kuka KR5 robot. This case also has issues with orientation constraints.

In the video I experimented with rviz_tools_py to visualize the planning problem before showing the solution. (Without having to edit video's.) It doesn't work that well, but maybe already better than text in the video description :) @felixvd

[felixvd]

Interesting package! Definitely looks better, but if you're already displaying it manually, try publishing the start and goal state as a RobotState (maybe colored green and gold to keep the same color scheme as the MotionPlanning plugin) and setting the alpha of the trajectory to 1.0. And naturally leave out the pose that isn't part of the planning problem, that's confusing. Should look real good.

[JeroenDM]

try publishing the start and goal state as a RobotState (maybe colored green and gold to keep the same color scheme as the MotionPlanning plugin) ...

That was my initial idea, but the python interface for visualizing stuff is Rviz is not ideal yet, and it was getting late :) I'll keep this in mind. It would be great if moveit_visual_tools is ported to Python, or does this already exist and did I miss it?

[mamoll]

I wouldn't call my approach to sampling nearby orientations esoteric. I prefer "correct" :-). If you want uniform sampling of orientations that are _at most_ some distance _d_ away, that's the way to do it. If you want orientations that are _exactly_ distance _d_ away, then skip the pow(...) term in the code you linked to. You could create a polyhedral approximation by just taking the convex hull of a few quaternions that are distance _d_ away. (This is analogous to how friction cones in 3D are often approximated by a polyhedron.) Checking whether a given orientation (4-vector) is in the positive span for the selected quaternions is easy.

[felixvd]

@mamoll I didn't have a look at the details for this, but how would you constrain the rotation around a certain axis in quaternion space? Can the descartes use case (one axis almost unconstrained) be described? It's a very common constraint.

@JeroenDM Seconding that a Python version of moveit_visual_tools would be very useful, I just assumed there wasn't one.

[mamoll]

@felixvd Let the desired rotation axis be omega and the range of rotations be [theta_low, theta_high]. Then sampling any theta in that range, combined with omega will give you a valid rotation in axis-angle representation. Converting that to a unit quaternion is straightforward.

Normally, quaternion distance between two unit quaternions q0 and q1 is defined acos(q0 . q1), with "." being the vector inner product. By changing that to acos(q0^T x D x q1), where D is a symmetric positive definite matrix, you create an anisotropic distance measure. For a given q0, all the q1's that are within some distance threshold using this measure will form an ellipsoid. With an appropriately selected D, you should be able to model constrained motions around 1 axis or 2 axes (both would be degenerate ellipsoids).

[JeroenDM]

@mamoll

I wouldn't call my approach to sampling nearby orientations esoteric. I prefer "correct" :-).

:) Absolutely! I don't know why I wrote esoteric... It is the correct approach for uniform sampling, and a valid alternative to represent a constraint region. My current implementation never directly samples the constrained region.

@felixvd

Can the descartes use case (one axis almost unconstrained) be described?

For tolerance on rotation around a single axis, you can sample this angle uniformly. But with tolerance around more that one axis, the current implementation of descrates produces biased samples. The important question is then, does this bias influence the planner's performance. For constraints similar to the ones I want to describe, Berenson suggests it does not:

We observe that while our method ensures a uniform sampling in the bounds of Bw, it could produce a biased sampling in the subspace of constrained spatial displacements SE( 3) that Bw parameterizes. However, this bias has not had a significant impact on the runtime or success-rate of our algorithms.

It would be very interesing to validate this, by comparing both approaches.

[mamoll]

Sampling orientations that satisfy orientation constraints and projecting orientations onto the nearest orientations that satisfy orientation constraints are two different different things. You also need the latter when trying to "fix up" configurations along a path that violate the orientation constraint, right?

[mamoll]

If there's a simple implementation in Descartes that produces biased samples, then I'm fine if you use that for now. We can look it at it later whether that bias really matters.

[JeroenDM]

You also need the latter when trying to "fix up" configurations along a path that violate the orientation constraint, right?

@mamoll Should I be doing this? As far as I understand, the states from OMPL's solution should not violate the constraints as mentioned here.

Any motion plan generated using this augmented state space will satisfy the constraint, as the primitive operations used by motions planners (e.g., ompl::base::StateSpace::interpolate) automatically generate constraint satisfying states.

Or did I misinterpret your question?

[ommmid]

@JeroenDM maybe it is good to investigate more in details how interpolate() generates states between the start and end without violating constraints and explain it here.

Just to throw and idea:
What if you get the solution of only-position-constraint problem and examine its path for the orientation. If the orientations along the path are not too off compared to the orientation constraint, then break down the path into segments and put the orientation constraint at the end of each segment and then solve and use interpolate().

[JeroenDM]

@JeroenDM maybe it is good to investigate more in details how interpolate() generates states between the start and end without violating constraints and explain it here.

I looked into it a little bit, but it does not look trivial. Therefore it seemed sensible to first check if the interpolated solution does violate the constraints. I added a simple check for the solution path and it does not violate the constraints. So, if this check is correct, I don't have to worry about invalid paths for now.

@ommmid I do not entirely understand the idea. Is the approach you explained intended to handle problems with both position and orientation constraints?

then break down the path into segments and put the orientation constraint at the end of each segment

The current implementation has no concept of path segments or adding constraints at the end of a segment. The path_constraints are applied to the complete path. In other words, the constraints are used to model a new state space (ProjectedStateSpace) in which the standard OMPL planners can do there thing, without having to know how the constraints work.

[JeroenDM]

I started working on combining position and orientation constraints. OMPL's ConstraintIntersection class could be a nice way to achieve this. It would also allow support for an arbitrary number of positions or orientation constraints. (In addition, I could split up position constraints for each variable, instead of using infinite bounds on unconstraint variables.)

I created a simple working example outside MoveIt here. Although, for melodic I will have to use an older version of OMPL, related to these changes.

[JeroenDM]

After struggling with the ConstraintIntersection class a bit, I tried another simpler approach (but less flexible) to combine position and orientation constraints. I created a welding example script and hardcoded the constraints, where I ignore constraints on the rotation around the tool's z-axis.

When I make the constraint region to small, it is too slow or does not work yet. I will investigate this and explain it in more detail tomorrow. Here is already a quick preview.

For this type of problem with a low dimensional constrained manifold (6 degrees of freedom - 5 constraints = 1 dimension), it could be that this projection based planning is not ideal. I expect that using analytical inverse kinematics in the Constraint::project method could greatly improve performance for these low dimensional manifolds. But this is not trivial to implement and can only be applied to specific robots.

[JeroenDM]

I tried adding collision checking in a minimal way because most of the examples can be solved with simple Cartesian interpolation if there are no collision objects. I created a new branch add-collision-checking and the implementation is mostly copied from the existing interface in MoveIt.

I discovered that I have absolutely no idea how the planning_scene, planning_scene_interface, and planning_scene_monitor work to add collision objects. I still have a lot to learn.

Also, the results of a task with orientation constraints and a single collision object are not ideal yet :) I probably should add some workspace bounds.

[felixvd]

Which reminds me: Do we have an easy way in MoveIt to constrain only part of the robot (e.g. the end effector) to a bounded area? I found this thread, but it doesn't sound promising.

[v4hn]

On Fri, Jun 26, 2020 at 07:46:21AM -0700, Felix von Drigalski wrote:

Which reminds me: Do we have an easy way in MoveIt to constrain only part of the robot (e.g. the end effector) to a bounded area? I found this thread, but it doesn't sound promising.

The workspace concept does not even apply to the stationary robot at all... Something we might want to change at some point..

You can add path constraints with position regions for individual links.
I'm not sure whether the planners will actually handle it correctly though.

[JeroenDM]

You can add path constraints with position regions for individual links.
I'm not sure whether the planners will actually handle it correctly though.

Mmm, interesting, I don't know yet how the current OMPL interface handles constraints on arbitrary links. I would expect that it relies on the constraint_samplers in moveit_core. I should look into this in more detail.
In my current implementation, it is not to difficult to allow constraints on arbitrary links, I think.

[JeroenDM]

I created another example to check planning with position constraints and collision avoidance. This one is slow. Compiled in release mode it takes 10 to 100 seconds.

Any suggestions for non-industrial examples are welcome. A more diverse set of examples should hopefully lead to more flexible code. Maybe something with a humanoid robot?

[v4hn]

I would expect that it relies on the constraint_samplers

For goal constraints I would expect so. But if I remember correctly non-orientation path constraints will not change the sampling procedure.
I would expect simple rejection sampling for pure position path constraints on arbitrary links.

That is to say, I expect better from your approach. :-)

[mamoll]

That Kuka example looks pretty cool. I suspect planning is slow because you don't have an analytical Jacobian. However, for end-effector pose constraints you can get the constraint Jacobian terms from the RobotState via RobotState::getJacobian(). Using that inside PositionConstraint::jacobian and RPYConstraints::jacobian should speed things up a lot.

[JeroenDM]

@mamoll

For PositionConstraints, I use an analytical Jacobian at the moment, although there is some level of indirection and Eigen constructor calls that could make it slow.

1. The RobotState::getJacobian() is called in BaseConstraint::geometricJacobian.
2. The first three rows are extracted in the position constraints specific PositionConstraint::calcErrorJacobian.
3. Then the Jacobian is converted to take the bounds into account in the generic method BaseConstraint::jacobian.

It is the last one that I override for AngleAxisConstraints with the default numerical one in ompl::base::Constraint::jacobian.

Other performance notes:

• If I don't use the analytical Jacobian for position constraints in the Kuka problem with collision objects, it consistently times out at 100 seconds. ( And I did not wait longer.)
• Without the collision objects, the problem Kuka problem gets solved in under a second.

[JeroenDM]

For goal constraints I would expect so. But if I remember correctly non-orientation path constraints will not change the sampling procedure.
I would expect simple rejection sampling for pure position path constraints on arbitrary links.

It would be very interesting to implement direct sampling of (simple) path constraints, but still modeling the state using the joint values. This would be a new (and hopefully better) version of the current pose_model_state_space. The latter stores the state as an SE3StateSpace. I suspect it could be faster than OMPL's projection based constrained planning approach for problems with both position and orientation constraints.

@mamoll @felixvd @ommmid This could be the topic of the second phase of GSoC.

[JeroenDM]

I tried adding support for arbitrary orientation of the box that specifies the constraint region for position constraints. This kind of works, but not ideally. I quickly wrote down the formula used in a pdf.
position_constraints.pdf

[JeroenDM]

Meeting notes 29/06/2020

@felixvd @mamoll @ommmid

General remarks

• Use whatever latest version of MoveIt and OMPL I need to solve a problem quickly.
• Don't get distracted by direct sampling of constraints, for now, focus on my current approach with OMPL's constrained planner.
• Visualize constraints by coloring the robot depending on whether it satisfies the constraints in RVIZ (when moving it around with an interactive marker.)
• To have a better idea of what is working, create an example with a parameter that can be changed to create slight variations. Some of these variations work, others not. (Reproducible!)
• Don’t focus on multiple constraint regions for now. This could be done with the MoveIt Task Constructor in the future.
• Don’t add lazy goal sampling for now. (Where the planner selects a joint configuration for a given goal region in task space.) A fixed joint configuration goal is fine for now.
• Don't be sad if the planner is slow.

Plan for this week

• Figure out why OMPL’s sanity checks for the Jacobian do not work. Explain the sanity checks I did myself for the Jacobian.
• Organize the examples (in a table?). Start from a very simple example and build it up gradually. The table can have empty slots for aspirational examples.

[JeroenDM]

I created a script to make it easy to add different collision scenes to test different planning cases. For example, to load the collision geometry from panda_1.urdf:

rosrun elion_examples publish_collision_scene.py panda_1

I have some trouble understanding how to use this planning scene when planning, so I asked a question on ROS answers.

All this is in the context of creating an overview of the examples I tried and posted here in a very unorganized way.

---

Edit
I went for a completely different approach. All examples are now described in a JSON file, with new run instructions in the readme (of the devel branch). I have 10 different examples for now in elion_examples/config. I added some nice pictures and animations on the wiki. I'm especially happy with the example below, combining position constraints, orientation constraints, and collision avoidance. (Although there is still some funky stuff going on at the start of the path :) )
@mamoll @felixvd @ommmid

[JeroenDM]

Meeting notes 06/05/2020

Thank you for going through my work and providing feedback :) @mamoll @ommmid @felixvd

Done, last week I:

• Found out why OMPL sanity check failed.
• Organized the examples, they run from JSON files now.
• Fixed many smaller errors in analytical jacobian RPY and angle axis constraints.
• Reached enlightenment regarding parts of the PlanningSceneMonitor.

Some todo alternatives for next week:

• Keep diving deeper into the constraint formulation for a while. RPY or Quaternion constraints.
• Write a planning scene monitor tutorial.
• Pose goal sampling -> move towards usable planner.
• Fix long-standing orientation bug in the current interface.

Feedback and comments during the meeting:

• Keep in mind that I have to check whether a solution even exists for a planning example, so I don’t spend time debugging examples that could never be solved anyway.
• It would be valuable to provide an easy interface or good tutorial for z-rotation free orientation constraints. (Glass almost upright, but rotation around symmetry axis is allowed.) This is to difficult to specify for many users.
• Maybe try AtlasStateSpace and TangentBundleStateSpace. These could perform better for some of the planning examples. I only use ProjectedStateSpace at the moment. (There could be some theoretical issues with how I formulate the constraints now. E.g. are the bounds included or not. I can discuss this in detail with @mamoll if needed. Certainly interesting but maybe not the focus of this project.)
• Use different planners, everything is done with RRTConnect for now. This is also a way to test my implementation. In theory it should not make a difference in the rest of the code which planner I use.
• Some of the less ideal paths in the examples could be a direct consequence of the fact that RRTConnect does not optimize anything, it finds a feasible path. Try RRTStar? (But do not spend time on specialized cost functions, use the default one.)
• Rviz constraint visualization 2.0: use an interactive marker to move the robot inside the constraints and block moving outside of them.

Conclusion:

• Create a plan for integration into MoveIt (to be presented/discussed on the next MoveIt maintainer meeting).
• Convert some of my examples in tutorials. Good examples and tutorials are important!

[JeroenDM]

Until now, all examples used RRTConnect. I added some code so you can try other OMPL planners by setting the "planner_id" in the JSON file. I tried some other planners and this seems two work as expected.

OMPL also offers different ConstraintStateSpace implementations. Until now I used ProjectedStateSpace. AtlasStateSpace and TangentBundleStateSpace do not work for the current constraint formulation, throwing the exception:

Error:   ompl::base::AtlasStateSpace::newChart(): Failed because manifold looks degenerate here.

Rewriting position and orientation constraints as equality constraints allow us to use them. This gives interesting results for the difficult example kuka_collision_2.json. Using PRMstar in combination with AtlasStateSpace results in a reasonable solution within 10 seconds. With the ProjectedStateSpace, it takes 200 seconds to find a far-from-optimal solution.

For orientation constraints, I get the following warning while planning:

Warning: ompl::base::AtlasStateSpace::sampleUniform(): Took too long; returning center of a random chart.

I'm not sure what is making it slow, but I'll leave it at this and continue working on the MoveIt integration for now. The current demo code probably shows that it is possible and useful to do constrained planning with OMPL. Now I can start making it useable :)

[JeroenDM]

@v4hn Do you know why the ModelBasedStateSpace inherits from ompl::base::StateSpace, and not from RealVectorStateSpace. It seems this could avoid quite some code duplication. (Both originally written by @isucan .) I can't figure out why this would not work.

I'm not sure this would help me to integrate constrained planning, but it would at least make the current interface slightly easier to understand.

[mamoll]

@JeroenDM, the ModelBasedStateSpace::StateType is quite different from the RealVectorStateSpace::StateType. The constrained planning code in OMPL converts things internally to Eigen vectors, so maybe what you need is some helper method in the StateSpace base class that returns a "view" of a state as an Eigen::Map. For state spaces with states composed of a contiguous array of doubles (or with a fixed stride), that is easy to do. For everything else, you can throw an exception. @zkingston, any thoughts you want to add?

[v4hn]

That being said, (1) I believe MoveIt's defaults do not use the Eigen::Map approach yet, even if continuous joints sets are used. This could really speed things up according to some tests by Dave. And, (2), if changing state space can simplify the ompl wrapper, I'm all for it. The two might be exclusive though.

[mamoll]

_In general_ robot states cannot be mapped with Eigen::Map, but ModelBasedStateSpace::StateType contains a double*, just like RealVectorStateSpace::StateType. I am a little confused by @v4hn's comment. I think you are talking about something else? Avoiding allocs & copies are generally good ideas, but I am talking specifically here about providing the constrained motion planning framework within OMPL with the right hooks to be able to work with both RealVectorStateSpace and ModelBasedStateSpace.

[JeroenDM]

@mamoll

I am talking specifically here about providing the constrained motion planning framework within OMPL with the right hooks to be able to work with both RealVectorStateSpace and ModelBasedStateSpace.

Although this is what I am trying to do in the "long" run, I asked the question also in the context of @v4hn's comment:

if changing state space can simplify the ompl wrapper

---

But in the context of the constrained motion planning with OMPL, I was thinking about the idea of extending ModelBasedStateSpace to be a valid ompl::base::ConstrainedStateSpace. This would make the integration much easier, but I do not think it's possible, because you would end up with some weird multiple inheritance scenario.

In any case, I think simplifying the current interface would greatly improve my chance of success. This would require some tests to make sure I don't break anything. This, in turn, would require me to have a good idea of the requirements of the current interface. That's the main part where I'm lacking. Especially when it comes to multiple planning groups and subspaces.

I'm digitalizing all my notes from going to the current interface and summarizing what I tried so far. (With the next maintainer meeting in mind.)

[JeroenDM]

Some notes from going through the code of the current interface.

[zkingston]

@mamoll I don't think there is anything technically incorrect with having ModelBasedStateSpace inherit from RealVectorStateSpace. The constrained planning code really just relies on the double array within the state so it can use Eigen::Map, as we don't have a fancier way in OMPL of specifying that there is an underlying contiguous array for the state's real values. IIRC some other code might also play nicer if ModelBasedStateSpace is a RealVectorStateSpace, like BIT* or some of the KPIECE projections. See my ancient pull request (https://github.com/ros-planning/moveit/pull/897), I did this exact change to simplify interface stuff.

@JeroenDM
Extending ModelBasedStateSpace to ConstrainedStateSpace is also not a bad idea - you would just have to implement your own constrained integration code which could easily be cribbed from the ProjectedStateSpace.

[JeroenDM]

I kind of made it work in the current interface by ignoring all the code related to goal sampling and temporarily assuming a single joint goal constraint which is extracted manually in ModelBasedPlanningContext::configure.

I think the issue in the code related to goal sampling is the following. In many places a generic ompl::base::State gets cast to a ModelBasedStateSpace::StateType to access the joint values:

state->as<StateType>()->values

When the state that goes in has the type ConstrainedStateSpace::StateType, the casting goes wrong and bad things happen.

@JeroenDM
Extending ModelBasedStateSpace to ConstrainedStateSpace is also not a bad idea - you would just have to implement your own constrained integration code which could easily be cribbed from the ProjectedStateSpace.

This would solve the problem mentioned above, but I don't like the idea of copy-pasting code from OMPL if I can find a way to use it directly. (I assume this is what you mean by "cribbed", I've never heard that word in this context :) @zkingston )

[JeroenDM]

I came up with another option to solve the casting problem. We can add a third type of state space that inherits from ModelBasedStateSpace, let's call it ConstrainedPlanningStateSpace. (It is not a constrained state space, but a state space to be used when doing constrained planning.)
Now we can override copyToRobotState and implement it as:

void ompl_interface::ConstrainedPlanningStateSpace::copyToRobotState(moveit::core::RobotState& rstate,
                                                                     const ompl::base::State* state) const
{
  const Eigen::Map<Eigen::VectorXd>& x = *state->as<ompl::base::ConstrainedStateSpace::StateType>();
  rstate.setJointGroupPositions(spec_.joint_model_group_, x);
  rstate.update();
}

instead of the default implementation in ModelBasedStateSpace:

void ompl_interface::ModelBasedStateSpace::copyToRobotState(moveit::core::RobotState& rstate,
                                                            const ompl::base::State* state) const
{
  rstate.setJointGroupPositions(spec_.joint_model_group_, state->as<StateType>()->values);
  rstate.update();
}

This means I can revert my changes in ModelBasedPlanningContext::convertPath to use spec_.state_space_->copyToRobotState again. However, I'm struggling a bit to do the same for copyToOMPLState and copyJointToOMPLState.

What do you think of this idea? @v4hn @mamoll

edit: I found a more generic way that does not use the copy constructor conversion to been Eigen::map and also works for the other methods I was struggling with. Use the getState method on the ConstrainedStateSpace to access the underlying state and cast this to the appropriate type.

void ompl_interface::ConstrainedPlanningStateSpace::copyToRobotState(moveit::core::RobotState& rstate,
                                                                     const ompl::base::State* state) const
{
  rstate.setJointGroupPositions(
      spec_.joint_model_group_,
      state->as<ompl::base::ConstrainedStateSpace::StateType>()->getState()->as<StateType>()->values);
  rstate.update();
}

[JeroenDM]

The approach explained above seemed to work well! I can now use the existing goal sampling functionality, although I have yet to test pose goals. I'm coming up with a plan to organize the pull requests to introduce the changes. I hope to submit them tomorrow or Monday so we can start discussing actual code :)

[JeroenDM]

The action is moving away from this thread at the moment, but here is a quick update:

OMPL constrained planning integration

I split up the integration process is several smaller pieces. My plan is to continue the discussion specific to this integration in the main pull requests that explains this process: #2219. It still needs a lot of testing / benchmarking / examples to clearly show what the advantages and disadvantages of the new planning technique will be.

Tracking down difficult segfaults

I encountered some bugs that only showed up in release mode. I started to use valgrind like this:

valgrind --leak-check=full --track-origins=yes --xml=yes --xml-file="/home/jeroen/valout.xml"  $TEST_EXECUTABLE

To make it work, I also build OMPL from source now (version 1.5.0), because of this issue.
I ended up using address sanitizer from clang more that valgrind, building the tests with:

catkin build moveit_planners_ompl --no-deps --make-args tests --cmake-args -CMAKE_CXX_COMPILER=/usr/lib/ccache/clang++ -DCMAKE_CXX_FLAGS=-fsanitize=address

and running them:

ASAN_SYMBOLIZER_PATH=/usr/lib/llvm-6.0/bin/llvm-symbolizer $TEST_EXECUTABLE

Where the prefixed variable is needed to get meaningful file descriptions in the output. A small inconvenience, once all of the above is enabled and used, I cannot get it disabled anymore without cleaning and rebuilding the workspace (which can take +30 minutes, since I'm also building OMPL now).

Simplifying the current interface

I keep finding things I can remove / rewrite / simplify in the current interface, now that I'm getting familiar with it. (Although I'm strategically avoiding the PoseModelStateSpace for now.) One of the more interesting rewrites that took me a while to understand was in void ompl_interface::ModelBasedStateSpace::copyJointToOMPLState

Conclusion

I'm still having fun :) but I'm possibly doing too many things at once since I'm working on ~10 branches simultaneously...

[JeroenDM]

In the last meeting, we discussed that I should create more examples that show why adding the new capability would be a good idea. @mamoll @felixvd @ommmid

One of the original goals was the solve the joint space jumps in plans created by the default planners using the PoseModel. But after a little experimenting with simple examples, I'm not sure that the new planner will be a better alternative than the current enforce_joint_state_space hack. The best solution for this specific problem is still fixing the PoseModelStateSpace, I think.

There are of course many other exciting possibilities for specific types of constraints, mainly with small constrained regions where the enforce_joint_state_space hack (rejection sampling) underperforms. But I need more time to investigate this with good examples. I only have the welding example and another example with equality orientation constraints shown below.

(I wrote the above intermediate conclusion in the context of the upcomming MoveIt manipulation meeting. I've created some slides with short videos to support the discussion.)

[v4hn]

I'm not sure that the new planner will be a better alternative than the current enforce_joint_state_space hack.

I don't see why you would say that. By far the most frequent use-case for path constraints is orientation constraints with almost no tolerance. Rejection sampling (i.e. enforce_joint_state_space) is unusable in this case because the density of valid samples in joint space is 0. The gradient-projection-based technique does not at all suffer from this problem.

Could you elaborate a bit on why you disagree?

[JeroenDM]

The best solution for this specific problem is still fixing the PoseModelStateSpace, I think.

@v4hn pointed out that this is not what I mean with the solution I have in mind. It is not so much a fix, as it is a new state space that samples the constraints in Cartesian space and then coverts them to joint space to do planning completely in joint space. This of course heavily relies on inverse kinematics. Depending on which kinematics solver is used, the implementation could end up almost identical to what happens in OMPL's ProjectedStateSpace.

However, my intuition is that direct sampling of the constraints in Cartesian space could perform slightly better, as paths created with the ProjectedStateSpace tend to traverse the boundaries of the constraint region. I think this happens because when most random joint space samples are outside the constraints. The projection process stops once they reach the boundary of the constraints, and so many samples end up exactly on the boundary.

I'm not sure that the new planner will be a better alternative than the current enforce_joint_state_space hack.

I don't see why you would say that.

This is a preliminary reaction of me, to the issues of the new planner with orientation constraints, for which I could not see a clear solution. In hindsight, if we focus on a specific set of problems (glass upright, welding, ...) and do more extensive testing, a solution may present itself in the process. Or formulating it as exact equality constraints could perform even better, for problems where this is applicable.

Anyway, @v4hn thank you for disagreeing with me :) and for the feedback.

Also, @mamoll often mentioned that finding a way to visualize these orientation constraint boundaries could provide insight. So maybe it is time to spend some time on this.

[v4hn]

that samples the constraints in Cartesian space and then coverts them to joint space to do planning completely in joint space.

Depending on which kinematics solver is used, the implementation could end up almost identical to what happens in OMPL's ProjectedStateSpace.

Ironically, depending on what exactly you mean by your abstract planner description, you would probably either end up with something very similar to ProjectedStateSpace or with something very similar to PoseModelStateSpace. So to see what problems exactly would be resolved (and come up) with the method you have in mind, you would have to write down in some detail what you actually want to do.

Either way, the fundamental issues, if you want to call them that, on both ends are

1. projection ends up with most samples at the constraint boundary
2. and sampling in Constraint space with inverse mapping struggles with joint space completeness and continuity

Personally, I don't think 1. is such a big issue as you think. Did you already look at the results of optimizing planners like RRT*? I would imagine they would move away from the boundary over time.

For the box-constraint examples you gave, I was also vaguely wondering why OMPLs path shortcutting did not manage to shorten them. But of course it might be that shortening them in joint space is not as easy as it looks.

[felixvd]

By far the most frequent use-case for path constraints is orientation constraints with almost no tolerance.

I would argue that the most frequent real-world use-case is orientation constraints around two axes with one axis unconstrained. This is true for:

• Pouring from bottles
• Carrying open containers
• Grinding, polishing (the Descartes planner example)
• Welding
• Paint spraying
• Using screwdrivers, drills

Not a contradiction, just an addition. Having an easy way to define these constraints would be extremely useful.

[JeroenDM]

As discussed in the meeting, I will focus on the example @felixvd mentioned above. This should be a good starting point to get a minimal working example merged that is also useful in practice. The nice thing is that It can be modeled without all to code that handles bounds, and support AtlasStateSpace and TangentBundleStateSpace without extra work.

Some long overdue replies to @v4hn

Did you already look at the results of optimizing planners like RRT*?

I did, and the paths are indeed better. I don't have an example gif or something, and I must admit I did not test it extensively because I often don't have the patience to wait for the solution of these slower planners :s

I was also vaguely wondering why OMPLs path shortcutting did not manage to shorten them.

Interesting question. The examples I showed always animate an interpolated path, as the solution often only contains a couple of states. So this might be why it's difficult to see from the animation why the path shortcutting did not do more work. I'm not sure.

inverse mapping struggles with joint space completeness

This should not be a problem if you have analytical inverse kinematics, but of course, often you don't. Your analysis of the problem really helped to clarify the issues for me. You convinced me to focus on the projection-based techniques, at least for a while :)

[v4hn]

inverse mapping struggles with joint space completeness

This should not be a problem if you have analytical inverse kinematics, but of course, often you don't.

That's not the key issue I referred to. For simple almost-freespace planning problems you generally don't even need a fast solver, because the search trees are often not that big. (< 500 states)

If you map from Cartesian space to joint space, you either need to choose your joint solution based on some criteria (and thus ignore other solutions in this state) or end up solving a Decartes-style problem in your search graph on top of the actual planning graph - that is selecting the best sequence of IK solutions to get a continuous&smooth path in joint space when traversing the search graph.

Looking forward to a good upstream solution for the discussed 2d orientation constraints.
Sorry I didn't make it to today's meeting, I messed up time zone conversion and joined too late.

[JeroenDM]

For simple almost-freespace planning problems you generally don't even need a fast solver, because the search trees are often not that big. (< 500 states)

And collision checking would still be more of a bottleneck (runtime wise) that IK in that case probably... As a side note, I never thought about the fact that numerical inverse kinematics solvers are probabilistically complete, maybe. Given enough random seeds, they will find all existing solutions. (Although that would depend on the details of the IK solver.)

solving a Decartes-style problem in your search graph on top of the actual planning graph

I don't see how there are two different graphs in the picture. For example, in the case of PRM, you could add all IK solutions to the graph. Although we would have to come up with a state sampler that keeps states in a buffer to conform with OMPL's StateSampler class that only returns one state at a time.

It's interesting how "Descartes" has become the term for that specific planning approach. (Grid search with an explicit constraint model? GSECM?)

Sorry I didn't make it to today's meeting, I messed up time zone conversion and joined too late.

Feel free to join any other week, it is a weekly meeting. (Mostly a short update on what I did and will be doing.)
After I finished the first working example and I have some more data, we could continue the above discussion in one of these meetings.

[v4hn]

For example, in the case of PRM, you could add all IK solutions to the graph.

True. That's a way to handle it for <=6 dof.
If you have a continuous solution space for IK, or even just a huge number of solutions - the unconstrainted ur5 has up to has up to 256 solutions due to 4pi joint limits - this approach will generate heap of "irrelevant" states though.
Still an interesting approach to have around.

[JeroenDM]

generate heap of "irrelevant" states though.

That seems to be the core issue for any sampling-based planning algorithm that wants to reach some form of completeness. Sampling the redundant joints of a +6 dof robot is not that different from sampling joint configurations in general, except that you have to make an explicit choice of what joints are the redundant ones. (There is even support for this in KinematicsBase, although only ikfast implements it for a 1 redundant dof. At least for the solvers in the MoveIt repository.)

Still an interesting approach to have around.

Agreed. But probably not for this GSoC project anymore, unfortunately. It seems that someone else must have tried it (outside MoveIt), but I can't think of anyone right now. Otherwise, I'm putting it in my PhD :)

[v4hn]

That seems to be the core issue for any sampling-based planning algorithm that wants to reach some form of completeness

Yes and no. For PRM it doesn't make much of a difference indeed. RRT, et al. on the other hand, steers away from the growing tree and it changes behavior a lot if you systematically add all ik solutions for a sampled Cartesian pose.
Another way to approach this is of course to sample a single IK solution for a sampled Cartesian pose.

But yes, it's not this GSoC project. :-)

[JeroenDM]

I found another issue that was causing strange errors. The OMPL state pointers have to be cast correctly in the StateValidityChecker, taking into account that the state could be wrapped inside a ConstrainedStateSpace::StateType. I tried to fix this in this diff.

When doing this, I was wondering why OMPL Planners would call the isValid method several times for the same state. I checked, and the cached validity of states in MoveIt is used quite often. @mamoll

In general, I need a more organized way to make sure all the issues with casting the raw state pointers to the correct type are gone, I'm thinking about it.

[JeroenDM]

Yesterday I created a planning problem that could be a nice tutorial, based on a setup I can potentially do some real-world experiments up. Moving a gripper around some obstacle onto a block, while keeping it upright.

However, I'm having issues with the orientation constrained and did not manage to make the constrained planning approach work for this example, yet. (The default MoveIt planner did fine for this example.)

After solving some issues in the main pull request, as mentioned in the previous comment, I still have randomly occurring segfaults. I'm a bit stuck, as there are so many changes in that pull request it is difficult to isolate things. (Some of the smaller building blocks such as the ConstrainedPlanningStateSpace and BaseConstraint class are tested independently. The issue is probably somewhere in the planning_context_manager.cpp or model_based_planning_context.cpp.

I think I will start from a clean master branch again and start adding changes incrementally to see where it goes wrong.

[JeroenDM]

In the last meeting, we discussed that I should focus on systematically fixing all memory issues and other errors. (Instead of spending time on additional simplification of the current interface, or worrying about the performance.) I started from a clean branch and added the code to model position constraints.

The following OMPL sanity check error always comes up:

Sanity checks did not pass: State samplers generate constraint unsatisfying states.

After investigating this in more detail, (for a specific instance of position constraints on the end-effector), I came across an interesting issue with the uniform sampler of a constrained state space:

void ompl::base::ProjectedStateSampler::sampleUniform(State *state)
{
    WrapperStateSampler::sampleUniform(state);  // Get random state, inside joint limits
    constraint_->project(state);          // Iteratively project state on constraints (maximum 50 iters by default)
    space_->enforceBounds(state);  // change joint values that are outside the joint limits
}

As you can see in the code, the robot joint limits are applied after the state is projected onto the constraints. As it turns out, this means that many of the states returned by the sampler do not satisfy the constraints anymore.

The figures below show the success rate from generating 100 samples, and counting how many satisfy the constraints. I generated the samples for 49 different settings of constraint->setMaxIterations.

Before enforceBounds

After enforceBounds

Note: I used different position constraints for the panda robot, compared to the other 2 robots.
*Note 2: You can look at the data and generate the figures in this online notebook. To generate the data, I added wrote a C++ node in the elion package.

Solution

I could leave the path constraint check in the MoveIt StateValidityChecker, to make sure the invalid samples do not get accepted. (As @ommmid mentioned in a comment on the related pull request.)

But I think I'll leave this as is this week and first fix the other parts of the integration, in order to not lose sight of the whole picture.

@mamoll @felixvd

[JeroenDM]

I'm having trouble solving an issue in a test I'm creating to check if the StateValidityChecker works as expected. To make the integration work I will have to add some tricky type conversion code to it, so I want to make sure I understand how it works with some isolated tests.

This test should check the path constraints. When creating it, I came across a segmentation fault in geometric_shapes on this line. The call stack looks something like this:

  kinematic_constraints::KinematicConstraintSet ks(robot_model_);
  const moveit::core::Transforms& tfs = planning_scene_->getTransforms();

  // this is where it goes wrong
  ks.add(path_constraints, tfs);

(some other functions)

std::unique_ptr<shapes::Shape> shape(shapes::constructShapeFromMsg(...));
...
body->setDimensionsDirty(shape.get());

 inline void setDimensionsDirty(const shapes::Shape* shape)
  {
    useDimensions(shape);
  }

void bodies::Box::useDimensions(const shapes::Shape* shape)  // (x, y, z) = (length, width, height)
{
  const double* size = static_cast<const shapes::Box*>(shape)->size;
  length_ = size[0];
  width_ = size[1];
  height_ = size[2];
}

Of course, the memory issue can have its origin somewhere else, full valgrind output.

I feel like I'm missing something obvious... any ideas for how to figure this out? @v4hn

(Maybe a good night's sleep will do :) )

---

Edit: after debugging the issue some more with the help of @felixvd yesterday, and trying to reproduce what we found yesterday, I finally found the cause of this segmentation fault! It occurs when the ros workspace is note sourced... ( :( a beginner's mistake hidden under layers of setup complexity.)

Turns out debugging a roslaunch file in vscode using the ROS extension sources the workspace automatically, but the way I set up debugging my tests using the C++ extension did not source the workspace.

Conclusion: the correct way to debug a test in vscode is to create a temporary launch file that starts the test and debug it with the vscode ROS extension. (The attach method explained in the documentation is difficult to use as you somehow have to make the test node wait for the debugger to attach programmatically, which is not trivial.)

[JeroenDM]

I fixed two other issues today and it seems that I'm getting really close to a first working version. First of all, I changed my implementation of getAddressValueAtIndex (again). My assumption about what type of state pointer it would get was wrong. I put in a dynamic_cast for now, to make sure I can easily spot unexpected uses of this function:

double* ompl_interface::ConstrainedPlanningStateSpace::getValueAddressAtIndex(ompl::base::State* ompl_state,
                                                                              const unsigned int index) const
{
  if (index >= variable_count_)
    return nullptr;

  // use dynamic casting to make our debugging live easier for now
  auto casted_state = dynamic_cast<ompl_interface::ConstrainedPlanningStateSpace::StateType*>(ompl_state);
  if (casted_state)
  {
    return casted_state->values + index;
  }
  else
  {
    ROS_ERROR_STREAM( "ompl_interface::ConstrainedPlanningStateSpace::getValueAddressAtIndex: got unexpected state type!!.");
  }
  return nullptr;
}

This second issue was that the same instance of ompl_interface::PositionConstraint is used in several threads at once. It has a robot_state_ attribute that is modified by its methods. I applied the same technique used in the StateValidityChecker to make it threadsafe.

A simple planning problem for position constraints works now, but the issue related to the joint limits mentioned in a comment above still remains. @mamoll suggested trying to add the joint limtis as another type of ompl::base::Constraint and using the ompl::base::ConstraintIntersection class to combine it with the position constraints. However, the projection method does not support a number of constraints larger than the ambient dimension. It results in the informative exception messages:

ompl::base::Constraint(): Ambient and manifold dimensions must be positive.
ompl::base::Constraint(): Space is over constrained!

I have some ideas for how to solve this, but no good ones yet...

[JeroenDM]

I finished a first version of the tutorial (!) that explains how to plan with position constraints. It goes together with a new branch where I added all the new functionality in clean structured commits. This video shows a walkthrough of the tutorial. The two branches you need for this tutorial are:

• moveit/ompl-constrained-planning-v5
• moveit_tutorials/ompl-constrained-planning-v2

I rewrote the tutorial in Python, and am really happy with that choice! It's much faster now to try different variations of a planning problem. (Although the Python node does not detect ctrl-c yet when things go wrong...I need to fix that.)

First thing tomorrow, I will write the pull requests for these two branches :)

The tutorial and implementation do not include orientation constraints yet. There are many unresolved issues with them. The main issue is that the orientation parameterization that works well (exponential coordinates) was not compatible with MoveIt's interpretation of orientation constraints. But we still need the latter because of the joint limits issue discussed earlier. It would involve a bit of hacking to make them work together and make it useable.

I'm convinced planning along planes and lines is a valuable addition to MoveIt. I'll add some examples with collision objects to strengthen this point. More importantly, once the basics are merged, the possibilities for new types of constraints are endless! :)

@felixvd @mamoll @ommmid

[ommmid]

Just to make the log consistent here is the related PR in moveit_tutorial

I agree that adding the capability of planning along planes and lines are valuable. Especially when they are the tip of the iceberge, a foundation that is able to work with new types of constraints.

[JeroenDM]

@ommmid @mamoll @felixvd
For the final evaluation, I need to add a link to a short summary of the work I have done. I created a simple Github gist so I have a link that I can hopefully keep alive for a long time and quickly fix errors in if necessary. I looked at some examples of the previous years, and this seemed a reasonable way to write it.

(The blogpost for the MoveIt website will probably cover similar things, but with more detail and pictures.)