Different profile string in CompositeInstruction leads to totally diffenent planning results

[thume4zzzz]

Hi, I'm using trajopt to plan a trajectory subject to Cartesian waypoint constraints.
If I use the following codes to create the planning program, everything works fine.

// Create Program
CompositeInstruction program("DEFAULT", CompositeInstructionOrder::ORDERED, mi);

However, if I follow the example in basic_cartesian_example.cpp and use:

CompositeInstruction program(
      "cartesian_program", CompositeInstructionOrder::ORDERED, ManipulatorInfo("manipulator", "base_link", "tool0"));

the planner will fail because some of the waypoint constraints can not be satisfied.

Could someone tell me what is the difference?

[marip8]

In the code snippet from this example, we are configuring a TrajOpt profile that is specific to this application under the name cartesian_program, and it has different values than the default values of the TrajOptDefaultCompositeProfile class. Therefore, when you change the composite instruction to use the DEFAULT profile, you're telling the planner to apply various costs and constraints that may not really be appropriate

tesseract_planning/tesseract_examples/src/basic_cartesian_example.cpp

Lines 224 to 231 in 1c6104e

	auto composite_profile = std::make_shared<TrajOptDefaultCompositeProfile>();
	composite_profile->collision_cost_config.enabled = true;
	composite_profile->collision_constraint_config.enabled = true;
	composite_profile->smooth_velocities = true;
	composite_profile->smooth_accelerations = false;
	composite_profile->smooth_jerks = false;
	composite_profile->velocity_coeff = Eigen::VectorXd::Ones(1);
	profiles->addProfile<TrajOptCompositeProfile>(TRAJOPT_DEFAULT_NAMESPACE, "cartesian_program", composite_profile);

If you could post the full TrajOpt log from the last iteration of the optimization, we may be able to identify what costs/constraints were causing the problem

[thume4zzzz]

In the code snippet from this example, we are configuring a TrajOpt profile that is specific to this application under the name cartesian_program, and it has different values than the default values of the TrajOptDefaultCompositeProfile class. Therefore, when you change the composite instruction to use the DEFAULT profile, you're telling the planner to apply various costs and constraints that may not really be appropriate

tesseract_planning/tesseract_examples/src/basic_cartesian_example.cpp

Lines 224 to 231 in 1c6104e

	auto composite_profile = std::make_shared<TrajOptDefaultCompositeProfile>();
	composite_profile->collision_cost_config.enabled = true;
	composite_profile->collision_constraint_config.enabled = true;
	composite_profile->smooth_velocities = true;
	composite_profile->smooth_accelerations = false;
	composite_profile->smooth_jerks = false;
	composite_profile->velocity_coeff = Eigen::VectorXd::Ones(1);
	profiles->addProfile<TrajOptCompositeProfile>(TRAJOPT_DEFAULT_NAMESPACE, "cartesian_program", composite_profile);

If you could post the full TrajOpt log from the last iteration of the optimization, we may be able to identify what costs/constraints were causing the problem

Thank you for your reply. I create a custom cartesian_program just like the example, now it works well. Here is another question. I wrote a test program that commands the manipulator to follow a circular path in Cartesian space.

  int point_num = 11;
  Eigen::Vector3d center(x,y,z);
  for (std::size_t i = 0; i < point_num; ++i)
  {
    theta =  2*M_PI / (point_num - 1) * i ;

    Eigen::Vector3d end(x-r*sin(theta), y, z+r*cos(theta));
    Eigen::Vector3d norm_z = (end - center).normalized();
    Eigen::Vector3d x_axis(0, 1, 0); 
    Eigen::Vector3d y_axis = (norm_z.cross(x_axis)).normalized();
    Eigen::Isometry3d pose;
    pose.matrix().col(0).head<3>() = x_axis;
    pose.matrix().col(1).head<3>() = y_axis;
    pose.matrix().col(2).head<3>() = norm_z;
    pose.matrix().col(3).head<3>() = end;

    CartesianWaypointPoly wp{ CartesianWaypoint(pose) };
    MoveInstruction plan_instruction(wp, MoveInstructionType::FREESPACE, "freespace_profile");
    plan_instruction.setDescription("waypoint_" + std::to_string(i));
    program.appendMoveInstruction(plan_instruction);
  }

The planner can correctly solve the trajectory. However, if I change the point_num to 10 or 20, the planner will fail to find a solution. What is the problem?

[thume4zzzz]

Here is the TrajOpt log from the last iteration of the optimization. The cartesian poses of planned trajectory deviate greatly from the set values.
log.txt

[marip8]

Here is the TrajOpt log from the last iteration of the optimization. The cartesian poses of planned trajectory deviate greatly from the set values.

A few questions:

• Is this log from your custom application you described above or from basic_cartesian_example.cpp?
• Are you using the TrajOpt IFOPT planner or the "regular" TrajOpt planner? What version of the trajopt repo are you on?
  • I have only used the "regular" TrajOpt planner before, and I have not seen it create joint velocity costs for every set of decision variables as in your log; I also don't recognize the names of the costs and constraints

The optimization seems to have failed because all of the cartesian position costs (CartPos_JointPosition in the new_exact column) are very large but should be zero.

My guess as to why this fails is that you might not have given TrajOpt a good joint seed for those Cartesian positions, so it got stuck and wasn't able to find a way to meet those constraints. I would recommend adding a SimplePlanner step before going to TrajOpt. This planner will solve IK for each Cartesian waypoint and set that IK solution as the seed for TrajOpt. Then TrajOpt can optimize that IK solution (if your robot has more than 6 DoF) and will likely be more successful

[thume4zzzz]

Here is the TrajOpt log from the last iteration of the optimization. The cartesian poses of planned trajectory deviate greatly from the set values.

A few questions:

* Is this log from your custom application you described above or from `basic_cartesian_example.cpp`?

* Are you using the TrajOpt IFOPT planner or the "regular" TrajOpt planner? What version of the `trajopt` repo are you on?
  
  * I have only used the "regular" TrajOpt planner before, and I have not seen it create joint velocity costs for every set of decision variables as in your log; I also don't recognize the names of the costs and constraints

The optimization seems to have failed because all of the cartesian position costs (CartPos_JointPosition in the new_exact column) are very large but should be zero.

My guess as to why this fails is that you might not have given TrajOpt a good joint seed for those Cartesian positions, so it got stuck and wasn't able to find a way to meet those constraints. I would recommend adding a SimplePlanner step before going to TrajOpt. This planner will solve IK for each Cartesian waypoint and set that IK solution as the seed for TrajOpt. Then TrajOpt can optimize that IK solution (if your robot has more than 6 DoF) and will likely be more successful

• This log is from my custom application based on basic_cartesian_example.cpp (use the same cartesian_program).
• I'm using TrajOpt IFOPT and the latest version trajopt repo (0.6.1).

Could you please give an example how to add a SimplePlanner?

[thume4zzzz]

Here is the TrajOpt log from the last iteration of the optimization. The cartesian poses of planned trajectory deviate greatly from the set values.

A few questions:

* Is this log from your custom application you described above or from `basic_cartesian_example.cpp`?

* Are you using the TrajOpt IFOPT planner or the "regular" TrajOpt planner? What version of the `trajopt` repo are you on?
  
  * I have only used the "regular" TrajOpt planner before, and I have not seen it create joint velocity costs for every set of decision variables as in your log; I also don't recognize the names of the costs and constraints

The optimization seems to have failed because all of the cartesian position costs (CartPos_JointPosition in the new_exact column) are very large but should be zero.
My guess as to why this fails is that you might not have given TrajOpt a good joint seed for those Cartesian positions, so it got stuck and wasn't able to find a way to meet those constraints. I would recommend adding a SimplePlanner step before going to TrajOpt. This planner will solve IK for each Cartesian waypoint and set that IK solution as the seed for TrajOpt. Then TrajOpt can optimize that IK solution (if your robot has more than 6 DoF) and will likely be more successful

* This log is from my custom application based on basic_cartesian_example.cpp (use the same cartesian_program).

* I'm using TrajOpt IFOPT and the latest version trajopt repo (0.6.1).

Could you please give an example how to add a SimplePlanner?

Update:
I follow the example in chain_example.cpp which utilizes descartes to get the seed for trajopt. But descartes fails to get monitor environment information. It seems like that ik solver (kdl is used) fails to get any available solution in some waypoints, but the given waypoints are definitely reachable.
descartes_log.txt
My robot is redundant so opw ik solver is not available.

[marip8]

If your robot is redundant and you are not discretely sampling the extra DoF, the descartes planner is not going to provide any additional benefit over the simple planner (i.e., interpolation and IK solving). If you notice in the descartes log, no waypoint (rung) in your plan has more than one IK solution (node), meaning that if all your waypoints were feasible there would only be one path through the graph and it would be equivalent to sequentially solving IK.

It appears the example is using a pipeline that only includes TrajOpt IFOPT. To use the simple planner in conjunction with TrajOpt IFOPT, you'll have to make a custom pipeline that is similar to this one. It would probably look like this:

 TrajOptIfoptTask:
        class: PipelineTaskFactory
        config:
          conditional: true
          inputs: [input_data]
          outputs: [output_data]
          nodes:
            DoneTask:
              class: DoneTaskFactory
              config:
                conditional: false
            ErrorTask:
              class: ErrorTaskFactory
              config:
                conditional: false
            MinLengthTask:
              class: MinLengthTaskFactory
              config:
                conditional: true
                inputs: [input_data]
                outputs: [output_data]
+            SimpleMotionPlannerTask:
+              class: SimpleMotionPlannerTaskFactory
+               config:
+                conditional: true
+                inputs: [input_data]
+                outputs: [output_data]
+                format_result_as_input: true
            TrajOptIfoptMotionPlannerTask:
              class: TrajOptIfoptMotionPlannerTaskFactory
              config:
                conditional: true
                inputs: [output_data]
                outputs: [output_data]
                format_result_as_input: false
            DiscreteContactCheckTask:
              class: DiscreteContactCheckTaskFactory
              config:
                conditional: true
                inputs: [output_data]
            IterativeSplineParameterizationTask:
              class: IterativeSplineParameterizationTaskFactory
              config:
                conditional: true
                inputs: [output_data]
                outputs: [output_data]
          edges:
+           - source: SimpleMotionPlannerTask
+             destinations: [ErrorTask, MinLengthTask]
            - source: MinLengthTask
              destinations: [ErrorTask, TrajOptIfoptMotionPlannerTask]
            - source: TrajOptIfoptMotionPlannerTask
              destinations: [ErrorTask, DiscreteContactCheckTask]
            - source: DiscreteContactCheckTask
              destinations: [ErrorTask, IterativeSplineParameterizationTask]
            - source: IterativeSplineParameterizationTask
              destinations: [ErrorTask, DoneTask]
          terminals: [ErrorTask, DoneTask]

The simple planner has a few profiles that determine how it performs interpolation. Either the LVS (default) or fixed size plan profiles should work here

[thume4zzzz]

Thank you for replying.
I have modified the yaml file according to your advice. But how to add a simple planner in the application? I use the following code :

  auto profiles = std::make_shared<ProfileDictionary>();

  // Simple planner profiles 
  auto simple_planner_profile = std::make_shared<SimplePlannerLVSPlanProfile>();
  profiles->addProfile<SimplePlannerPlanProfile>(SIMPLE_DEFAULT_NAMESPACE, "DEFAULT", simple_planner_profile);

  // Trajopt profiles 
  auto composite_profile = std::make_shared<TrajOptIfoptDefaultCompositeProfile>();
  composite_profile->collision_cost_config->type = tesseract_collision::CollisionEvaluatorType::LVS_DISCRETE;
  composite_profile->collision_constraint_config->type = tesseract_collision::CollisionEvaluatorType::LVS_DISCRETE;
  composite_profile->smooth_velocities = true;
  composite_profile->smooth_accelerations = false;
  composite_profile->smooth_jerks = false;
  composite_profile->velocity_coeff = Eigen::VectorXd::Ones(1);
  profiles->addProfile<TrajOptIfoptCompositeProfile>(
      TRAJOPT_IFOPT_DEFAULT_NAMESPACE, "cartesian_program", composite_profile);

  auto plan_profile = std::make_shared<TrajOptIfoptDefaultPlanProfile>();
  plan_profile->cartesian_coeff = Eigen::VectorXd::Ones(6);
  plan_profile->joint_coeff = Eigen::VectorXd::Ones(numJoints);
  profiles->addProfile<TrajOptIfoptPlanProfile>(TRAJOPT_IFOPT_DEFAULT_NAMESPACE, "RASTER", plan_profile);
  profiles->addProfile<TrajOptIfoptPlanProfile>(TRAJOPT_IFOPT_DEFAULT_NAMESPACE, "freespace_profile", plan_profile);
  
  // Create task
  const std::string task_name = "TrajOptIfoptTask";
  TaskComposerNode::UPtr task = factory.createTaskComposerNode(task_name);
  const std::string output_key = task->getOutputKeys().front();

  // Create Task Composer Problem
  auto problem = std::make_unique<PlanningTaskComposerProblem>(env_, profiles);
  problem->input_instruction = program;

but get
Error: Input instructions to MotionPlannerTask: SimpleMotionPlannerTask must be a composite instruction

[marip8]

But how to add a simple planner in the application?

The simple planner is already a part of the TrajOptIfoptTask "pipeline", so it get's added as a part of TaskComposerNode::UPtr task = factory.createTaskComposerNode("TrajOptIfoptTask");.

Error: Input instructions to MotionPlannerTask: SimpleMotionPlannerTask must be a composite instruction

This is exactly the problem described in #399. It appears that your problem is set up incorrectly. Per the discussion on that issue, the better way to solve this issue is to add a ProcessInputTask which looks at problem->input_instruction and makes sure that the information gets set in the correct place for the first task in the underlying DataStorage object:

 TrajOptIfoptTask:
        class: PipelineTaskFactory
        config:
          conditional: true
          inputs: [input_data]
          outputs: [output_data]
          nodes:
            DoneTask:
              class: DoneTaskFactory
              config:
                conditional: false
            ErrorTask:
              class: ErrorTaskFactory
              config:
                conditional: false
+            ProcessInputTask:
+              class: ProcessPlanningInputTaskFactory
+              config:
+                conditional: false
+                outputs: [input_data]
            MinLengthTask:
              class: MinLengthTaskFactory
              config:
                conditional: true
                inputs: [input_data]
                outputs: [output_data]
+            SimpleMotionPlannerTask:
+              class: SimpleMotionPlannerTaskFactory
+               config:
+                conditional: true
+                inputs: [input_data]
+                outputs: [output_data]
+                format_result_as_input: true
            TrajOptIfoptMotionPlannerTask:
              class: TrajOptIfoptMotionPlannerTaskFactory
              config:
                conditional: true
                inputs: [output_data]
                outputs: [output_data]
                format_result_as_input: false
            DiscreteContactCheckTask:
              class: DiscreteContactCheckTaskFactory
              config:
                conditional: true
                inputs: [output_data]
            IterativeSplineParameterizationTask:
              class: IterativeSplineParameterizationTaskFactory
              config:
                conditional: true
                inputs: [output_data]
                outputs: [output_data]
          edges:
+           - source: ProcessInputTask
+           - destinations: [SimpleMotionPlannerTask]
+           - source: SimpleMotionPlannerTask
+             destinations: [ErrorTask, MinLengthTask]
            - source: MinLengthTask
              destinations: [ErrorTask, TrajOptIfoptMotionPlannerTask]
            - source: TrajOptIfoptMotionPlannerTask
              destinations: [ErrorTask, DiscreteContactCheckTask]
            - source: DiscreteContactCheckTask
              destinations: [ErrorTask, IterativeSplineParameterizationTask]
            - source: IterativeSplineParameterizationTask
              destinations: [ErrorTask, DoneTask]
          terminals: [ErrorTask, DoneTask]

[thume4zzzz]

It seems like that KDL fails to generate joint solution for a reachable cartesian pose. Is it possible to switch to a different ik solver?

[marip8]

It seems like that KDL fails to generate joint solution for a reachable cartesian pose. Is it possible to switch to a different ik solver?

Depends on your robot. If it is a 6-DOF spherical wrist robot, you could use the OPW solver. If it's a UR, you can use the UR solvers. Otherwise, the KDL solvers are really the only option at the moment. You could update the seed state in your IK call to be closer to the final solution; that typically helps a lot with making the solver converge

[thume4zzzz]

Thank you for you reply. Finally I figure it out by changing the seed for KDL in each IK call. I also turn off collision check in Descartes. Now Descartes + trajopt work well when
trajopt_composite_profile->collision_cost_config.enabled = false;

But if I turn the collision cost on, trajopt will get stuck in the first Iteration.

It seems like it takes much time to convexify and process collision constraints. Shall I do VHACD decomposition and generate convex hulls for collision objects first?