Not able to solve Cartesian Plan

[bi3ri]

Hi, I am trying to compute a trajectory with trajopt to spray a isolation on a copper coil. The shape of the coil is quite complex, which leads to complex poses for the trajectory.

Not all poses can be reached by the robot perfectly, which is not a big problem as the spraying process has quite a big margin for errors. My problem is that I can not find any valid solution with trajopt, although I set the CartesianCoefficients very small. It seems that trajopt, ignores the CartesianCoefficients I set and still tries to find a solution without an error margin. I also tried them as CNT and Cost functions. Even setting the CartesianCoefficients to 0 seems to have no effect.

If I hack my algorithm in a way that the poses are reachable, I can solve the problem. But this only works for a tiny fraction of the path.

This is a trajectory computed with the hacked algorithm and what I would expect as an outcome from the original algorithm.

I created a small sample project with the poses and urdf here.

Any help or hint on how I could solve this would be highly appreciated! :)

Thanks! Johannes

[Levi-Armstrong]

I have a questions. Are the points not reachable due to kinematics or are they not reachable due to collision? If they are not reachable due to collision then we switch to a cost versus a constraint it usually works well.

[bi3ri]

In the end, I tried to find a solution with collision checking turned off. So the problem should lie in not reachable poses due to kinematics.

The "hacked" algorithm changes the poses in a way that y is showing up (in z direction) like the tcp, so the poses can be reached.

As I set the last CartesianCoefficient to 0, shouldn't Trajopt be able to find a valid solution that is free in the z-axis so the unreachable poses would become reachable?

[Levi-Armstrong]

Are you using the raster process planner or are you using the trajopt process planner?

[bi3ri]

I used the puzzle piece example as a draft and there the trajopt process planner was used.

Do you think switching to the raster process planner would have a higher chance of succeeding? I actually would prefer that the robot is continuing a linear movement between the rasters instead of planning free space. Starting and stopping the spray process has a delay of some seconds in my application.

[Levi-Armstrong]

The challenge with TrajOpt is it can get stuck in a local minimum so it needs a decent guess / start state to be successful. Currently, the default simple planner is not very good at this so if you use either the raster or cartesian process planner it will run descartes followed by trajopt which will produce much better results.

[bi3ri]

Yeah, I remember that you Jonathan Meyer talked about this in the presentation at RosCon 2018. A boring trajectory is a good trajectory ; )

The problem here is again that the poses are not reachable and that tesseract is not supporting a toleranced cartesian waypoint for descartes_light. I already tried and adapted the chain example of the tesseract_motion_planner package, but more than half to the vertices and edges are failing.

I also tried the cartesian process planner just now with the same outcome.

[Levi-Armstrong]

I see. Yea for my use cases it has always been collision not allowing it to reach the waypoint so descartes would pass with allowed collision. You could create a descartes profile that supports the tolerance.

[bi3ri]

I thought about that, but descartes_light which tesseract uses is not supporting toleranced cartesian waypoints either.

I am only working on my project thesis here and kinda already exceeded the time plan.

Still thanks for the insights!

[Levi-Armstrong]

For future reference, Descartes supports the ability to assign a custom target sampler. So you could create one that also samples position along with rotation.

 descartes_plan_profile->target_pose_sampler = [](const Eigen::Isometry3d& pose) {
    return tesseract_planning::sampleToolZAxis(pose, M_PI / 6.0);
  };

[bi3ri]

Oh, sorry I did miss that option! Adapting the original sampler to my needs was very easy and Descartes finds a solution now!

Now I am facing another Issue tough. I am not able to use the descartes_plan_profile with the custom sampler with a ProcessPlanningServer. If I don't use the ProcessPlanningServer like in the unit tests I don't face any issues.

If I use the ProcessPlanningServer it always uses the sampleFixed solver.

To reproduce I adapted the basic cartesian example. Just add the following lines here:

  //line 50
  #include <tesseract_motion_planners/descartes/profile/descartes_default_plan_profile.h>

  //line 218
  auto descartes_plan_profile = std::make_shared<tesseract_planning::DescartesDefaultPlanProfileD>();

  descartes_plan_profile->target_pose_sampler = [](const Eigen::Isometry3d& tool_pose) {
    tesseract_common::VectorIsometry3d samples;
    Eigen::Isometry3d p = tool_pose * Eigen::AngleAxisd(M_PI/2, Eigen::Vector3d::UnitY());
    samples.push_back(p);
    return samples;
  };

  planning_server.getProfiles()->addProfile<tesseract_planning::DescartesDefaultPlanProfileD>("RASTER", descartes_plan_profile);

Did I understand something wrong? With the new sampler, I would expect the tool poses to be rotated around the Y axis with 90 degrees, but nothing changes.

[Levi-Armstrong]

Ah, you need to add it to the profiles using the base class type, not the inherited type. We need to figure out a way to check when adding profiles to make sure the type is the base type. In the next week or so we will be looking into if we can leverage a base class to solve this issue, because this happens often.

[bi3ri]

Uh, I thought pretty long that I would confuse something with the names, as there are three a profile name, a program name, and a taskflow name (which is named new_planner_name as a variable in the examples).

Maybe you could overload the addProfile() function instead of using a template, this would make it typesafe.

Also, even the naming could be made typesafe, by a strong typedef from boost and look maybe like this: tesseract_common::profile_name raster_profile("RASTER");

EDIT: What would be the base class? I tried the following, with no effect: planning_server.getProfiles()->addProfile<tesseract_planning::DescartesPlanProfile<double>>("RASTER", descartes_plan_profile);

Also if I understand it right, the request in the example is wrong. Shouldn't the request be a for a Cartesian Taskflow: request.name = tesseract_planning::process_planner_names::CARTESIAN_PLANNER_NAME;

I tried both CARTESIAN_PLANNER_NAME and DESCARTES_PLANNER_NAME. Still with no effect.

[Levi-Armstrong]

What would be the base class? I tried the following, with no effect: planning_server.getProfiles()->addProfile<tesseract_planning::DescartesPlanProfile<double>>("RASTER", descartes_plan_profile);

Switch to using float instead of double.

[rjoomen]

I think this can be closed.

[Levi-Armstrong]

Please reopen if needed.