TrajOpt failed to find a collision-free path in the following example

[xibeisiber]

Hi,
I am testing with a simple example modified from basic_cartesian_example: move the manipulator from one side of a cylinder to the other side in joint space:

But trajOpt failed to find a valid path. I'm not sure which paramter should be tuned.

The ros package "test_example" is attached. It needs xacro and srdf files from tesseract_ros_example.

Usage: (method_id: 1: trajopt_ifopt profile; 2: trajopt profile; 3: ompl profile)
roslaunch test_example test_example.launch method_id:=3
method_id 1&3 can find solution in about 20~30s. method_id 2 fails to find a solution.

test_example.zip

Thanks!

[xibeisiber]

solution is found when ifopt is enabled. but it takes about 23s.

[Levi-Armstrong]

What hash are you using for tesseract repos?

[marip8]

Are there waypoints that intersect with the collision column? If so, the current default profiles for the TrajOpt and TrajOpt-IFOPT planner add Cartesian waypoints as constraints (rather than costs). From what I understand, this means that the robot must visit each waypoint exactly (since you are specifying a cartesian_coeff of a vector of ones) regardless of the other costs in the problem like collision

[xibeisiber]

What hash are you using for tesseract repos?

tesseract: commit 1241d51a953ff655082c77878ec82b33d12b9070 Sat Dec 4 00:53:36 2021 -0600
tesseract_planning: commit e4afe13b643f8df0a02d462a908071d6515c2f04 Mon Dec 6 15:15:26 2021 -0600
tesseract_ros: commit af0dc18ab456eebbfd5b7e87ae4f632b243739c4 Mon Dec 6 16:55:48 2021 -0600
trajopt: commit 4799fa2a276a9524b4f6f1ce388955e2722380a1 Sat Dec 4 10:36:54 2021 -0600

ubuntu 18.04, ros melodic
I updated the package test_example.zip with method_id: 1: trajopt_ifopt profile; 2: trajopt profile; 3: ompl profile
method_id 1 & 3 can find solutions in about 20~30s. method_id 2 cannot find a solution.

BTW: does the method with ompl profile use the solution found by ompl as initial seed of trajopt? I got output like this:
blabla
Info: RRTConnect: Created 15 states (7 start + 8 goal)
Debug: ParallelPlan.solveOne: Solution found by RRTConnect in 0.670333 seconds
Info: ParallelPlan::solve(): Solution found by one or more threads in 0.670418 seconds
[ERROR] convex solver failed! set TRAJOPT_LOG_THRESH=DEBUG to see solver output. saving model to /tmp/fail.lp and IIS to /tmp/fail.ilp
[ERROR] convex solver failed! set TRAJOPT_LOG_THRESH=DEBUG to see solver output. saving model to /tmp/fail.lp and IIS to /tmp/fail.ilp
[ERROR] convex solver failed! set TRAJOPT_LOG_THRESH=DEBUG to see solver output. saving model to /tmp/fail.lp and IIS to /tmp/fail.ilp
[ INFO] : Planning took 31.466352 seconds.

[xibeisiber]

Are there waypoints that intersect with the collision column? If so, the current default profiles for the TrajOpt and TrajOpt-IFOPT planner add Cartesian waypoints as constraints (rather than costs). From what I understand, this means that the robot must visit each waypoint exactly (since you are specifying a cartesian_coeff of a vector of ones) regardless of the other costs in the problem like collision

there is no such waypoint. Defined waypoints are:
wp0: joint_pos0=[0, 0, 0, 0, 0, 0, 0];
wp1: joint_pos1=[45, 45, 0, -45, 0, 0, 0] degree;
wp2: joint_pos2=[-45, 45, 0, -45, 0, 0, 0] degree;
paths: wp0 -> wp1 -> wp2 ->wp0
I comment out the cartesian_coeff in the updated test_example.zip with method_id: 1: trajopt_ifopt profile; 2: trajopt profile; 3: ompl profile.
method_id 1 & 3 can find solutions in about 20~30s. method_id 2 cannot find a solution.

[Levi-Armstrong]

I was able to recreate the issue. There is a fundamental difference between TrajOpt and TrajOptIfopt when it comes to how collision are added to the SQP problem. In the case of TrajOpt it adds every contact as a single equation which results in a large number of equations added. In the case of TrajOptIfopt the number of equation cannot be dynamic so we had to change things. If you set the num_eq to three it will find the three pairs of links which have the worst contact distance and generate a weighted average for gradient for a given link pair and use the worst contact distance as the constant when it creates the convex approximation. This appears to make TrajOptIfopt more stable when dealing with octomaps because for a given link pair that includes octomap you get a single equation that represents all of the contacts. This may mean it is better to update TrajOpt to do the same thing as TrajOptIfopt. I was able to get it to plan if you replace the function addPointCloud() with the one below.

Command::Ptr TestExample::addPointCloud()
{
  // Create octomap and add it to the local environment
  pcl::PointCloud<pcl::PointXYZ> full_cloud;
  double delta = 0.01;
  double height = cylinder_h_;
  double radius = 0.1;
  auto n_h = static_cast<int>(height / delta);
  auto n_r = static_cast<int>(radius / delta);
  auto n_th = static_cast<int>(2*3.1415926 / delta);

  for (int h = 0; h < n_h; ++h)
    for (int r = 0; r < n_r; ++r)
      for (int th = 0; th < n_th; ++th)
        full_cloud.push_back(pcl::PointXYZ( static_cast<float>(r * delta * cos(th*delta)),
                                            static_cast<float>(r * delta * sin(th*delta)),
                                            static_cast<float>(h * delta)) );

  sensor_msgs::PointCloud2 pointcloud_msg;
  pcl::toROSMsg(full_cloud, pointcloud_msg);

  // Add octomap to environment
  Link link_octomap("octomap_attached");

  Visual::Ptr visual = std::make_shared<Visual>();
  visual->origin = Eigen::Isometry3d::Identity();
  visual->origin.translation() = Eigen::Vector3d(0.8, 0, 0);
  visual->geometry = std::make_shared<tesseract_geometry::Octree>(full_cloud, 0.025, tesseract_geometry::Octree::SPHERE_INSIDE, true);
  link_octomap.visual.push_back(visual);

  Collision::Ptr collision = std::make_shared<Collision>();
  collision->origin = visual->origin;
  collision->geometry = visual->geometry;
  link_octomap.collision.push_back(collision);

  Joint joint_octomap("joint_octomap_attached");
  joint_octomap.parent_link_name = "base_link";
  joint_octomap.child_link_name = link_octomap.getName();
  joint_octomap.type = JointType::FIXED;

  return std::make_shared<tesseract_environment::AddLinkCommand>(link_octomap, joint_octomap);
}

[Levi-Armstrong]

Also, It is always good to set the collision buffer to a value greater than zero which makes things more stable and converge faster. Below I have set it to 2cm which will allow it to still include equation in the SQP problem near the collision margin.

    auto composite_profile = std::make_shared<TrajOptIfoptDefaultCompositeProfile>();
    composite_profile->collision_cost_config->type = tesseract_collision::CollisionEvaluatorType::LVS_DISCRETE;
    composite_profile->collision_cost_config->collision_margin_buffer = 0.02;
    composite_profile->collision_constraint_config->type = tesseract_collision::CollisionEvaluatorType::LVS_DISCRETE;
    composite_profile->collision_constraint_config->collision_margin_buffer = 0.02;

    auto composite_profile = std::make_shared<TrajOptDefaultCompositeProfile>();
    composite_profile->collision_cost_config.enabled = true;
    composite_profile->collision_cost_config.safety_margin_buffer = 0.02;
    composite_profile->collision_constraint_config.enabled = true;
    composite_profile->collision_constraint_config.safety_margin_buffer = 0.02;

[Levi-Armstrong]

Oh, forgot one thing. Also had to set the coeff for TrajOpt to 1 instead of the default which is 20. This is leading to believe I should update the original TrajOpt to handle collision the same way TrajOptIfopt.

   auto composite_profile = std::make_shared<TrajOptDefaultCompositeProfile>();
    composite_profile->collision_cost_config.enabled = true;
    composite_profile->collision_cost_config.safety_margin_buffer = 0.02;
    composite_profile->collision_cost_config.coeff = 1;
    composite_profile->collision_constraint_config.enabled = true;
    composite_profile->collision_constraint_config.safety_margin_buffer = 0.02;
    composite_profile->collision_constraint_config.coeff = 1;

[Levi-Armstrong]

Also motion planning is much faster if you sphere instead of boxes and recommend building in release because the default is debug.

[xibeisiber]

@Levi-Armstrong Thanks very much for your detailed answer. Trajopt and TrajoptIfopt now can find valid solutions within 2s now after implementing your suggestion.

maybe not so important: TrajoptIfopt has the following error output while it did find a valid path :

Error:   Approximate merit function got worse (-7.451e-04). (convexification is probably wrong to zeroth order)
         at line 217 in /home/jia/trajplan_ws/src/trajopt/trajopt_optimizers/trajopt_sqp/src/trust_region_sqp_solver.cpp

[marip8]

If you set the num_eq to three it will find the three pairs of links which have the worst contact distance and generate a weighted average for gradient for a given link pair and use the worst contact distance as the constant when it creates the convex approximation.

Is the option for using a single weighted sum of all collision vectors still an option in both the TrajOpt and TrajOptIfopt planners? It seems like that would be a good alternative to specifying an arbitrary number of collision vectors to consider

[Levi-Armstrong]

@Levi-Armstrong Thanks very much for your detailed answer. Trajopt and TrajoptIfopt now can find valid solutions within 2s now after implementing your suggestion.

maybe not so important: TrajoptIfopt has the following error output while it did find a valid path :

Error:   Approximate merit function got worse (-7.451e-04). (convexification is probably wrong to zeroth order)
         at line 217 in /home/jia/trajplan_ws/src/trajopt/trajopt_optimizers/trajopt_sqp/src/trust_region_sqp_solver.cpp

This will usually happen near a convergence. It should be changed to a warning. If you were to add your constraint/cost then and you were seeing this on the first few iteration then you would want to look into it.

[Levi-Armstrong]

If you set the num_eq to three it will find the three pairs of links which have the worst contact distance and generate a weighted average for gradient for a given link pair and use the worst contact distance as the constant when it creates the convex approximation.

Is the option for using a single weighted sum of all collision vectors still an option in both the TrajOpt and TrajOptIfopt planners? It seems like that would be a good alternative to specifying an arbitrary number of collision vectors to consider

The legacy trajopt has an option to enable weighted average, but I know for a fact how it is implemented is incorrect. I correctly implemented it in the IFOPT version but did not with trajopt because the goal is that it will be deprecated once we are confident that IFOPT version is ready. I do not believe it would be a lot of work to update trajopt to do the same.

[marip8]

The legacy trajopt has an option to enable weighted average, but I know for a fact how it is implemented is incorrect.

I might look into back-porting this for the legacy planner since we're using this option for one of my current projects

[Levi-Armstrong]

@marip8 Here is a good place to start and where it is used.

[xibeisiber]

When the robot is closer to the collision cylinder (say 0.7), both trajopt and trajopt_ifopt failed to find a collision-free path, while ompl X trajopt succeeded.

ProcessPlanningFuture response = planning_server.run(request);
I saw the planning result is saved in a structure ProcessPlanningFuture, is there a bool flag indicating whether the result is feasible and collision-free?

[Levi-Armstrong]

Here is an example in the ros planning server on how to check if it was successful.

When the robot is closer to the collision cylinder (say 0.7)

Is this the base of the robot? If so, one thing to remember is that trajopt is a local optimizer, so the initial condition(trajectory) will affect its ability to find a solution. This is why ompl + trajopt succeeds because ompl gave trajopt a better seed to start with allowing it to find a different local optimum solution. If you are using the default trajopt process planner it is using joint interpolated as the initial seed which get created by the Simple planner. In this case it may be better to change the simple planner the fixed state assign simple planner and tell it to assign all states in the trajectory as the start state. This will allow trajopt to evolve organically from the start to the goal.

I image why it fails when you move it closer is that joint 6 link gets a gradient to move way from the base of the robot and the other links get a gradient to move closer to the base creating opposing constraints at the optimizer.

  tesseract_common::Timer timer;
  timer.start();
  tesseract_planning::ProcessPlanningFuture plan_future = planning_server_->run(process_request);
  plan_future.wait();  // Wait for results
  timer.stop();


  result.response.successful = plan_future.interface->isSuccessful();
  result.response.results = Serialization::toArchiveStringXML<tesseract_planning::Instruction>(*(plan_future.results));

[xibeisiber]

Gotcha. Thanks very much!

[rjoomen]

The legacy trajopt has an option to enable weighted average, but I know for a fact how it is implemented is incorrect. I correctly implemented it in the IFOPT version but did not with trajopt because the goal is that it will be deprecated once we are confident that IFOPT version is ready. I do not believe it would be a lot of work to update trajopt to do the same.

Other question: has the weighted average been fixed in legacy trajopt?

[Levi-Armstrong]

Other question: has the weighted average been fixed in legacy trajopt?

It has not, but I have a PR which should completely fix it for TrajOptIfopt where each shape pair has an equation instead of a link pair because a link can have multiple shapes.

I think the best solution would be to copy the code from IFOPT into trajopt_utils that does the averaging and use it in both trajopt and trajopt_ifopt.