Generate more waypoints than I designed.

[zzzbbs]

Hi! I'm currently adding my robot to use trajopt using basic_cartesian_example. I want the robot to the first joint state and then back to the last joint state. But it seems the program will automatively generate states that I don't design. My codes are as follows:

  // Set the robot initial state
  std::vector<std::string> joint_names;
  joint_names.emplace_back("leg_joint1");
  joint_names.emplace_back("leg_joint2");
  joint_names.emplace_back("leg_joint3");
  joint_names.emplace_back("leg_joint4");
  joint_names.emplace_back("right_arm_joint1");
  joint_names.emplace_back("right_arm_joint2");
  joint_names.emplace_back("right_arm_joint3");
  joint_names.emplace_back("right_arm_joint4");
  joint_names.emplace_back("right_arm_joint5");
  joint_names.emplace_back("right_arm_joint6");
  joint_names.emplace_back("right_arm_joint7");

  Eigen::VectorXd joint_pos(11);
  joint_pos.setZero();
  joint_pos(0) = 0.0;
  joint_pos(1) = 0.0;
  joint_pos(2) = 0.0;
  joint_pos(3) = 0.0;
  joint_pos(4) = -0.842;
  joint_pos(5) = -1.309;
  joint_pos(6) = 0.227;
  joint_pos(7) = 1.561;
  joint_pos(8) = 0.051;
  joint_pos(9) = 0.754;
  joint_pos(10) = -1.264;

  env_->setState(joint_names, joint_pos);

  if (debug_)
    console_bridge::setLogLevel(console_bridge::LogLevel::CONSOLE_BRIDGE_LOG_DEBUG);
  else
    console_bridge::setLogLevel(console_bridge::LogLevel::CONSOLE_BRIDGE_LOG_INFO);

  // Create Task Composer Plugin Factory
  const std::string share_dir(TESSERACT_TASK_COMPOSER_DIR);
  tesseract_common::fs::path config_path(share_dir + "/config/task_composer_plugins.yaml");
  TaskComposerPluginFactory factory(config_path);

  // Create Program
  CompositeInstruction program(
      "cartesian_program", CompositeInstructionOrder::ORDERED, ManipulatorInfo("manipulator", "base_link", "right_arm_end_effector_mount_link"));

  // Start Joint Position for the program
  StateWaypointPoly wp0{ StateWaypoint(joint_names, joint_pos) };
  MoveInstruction start_instruction(wp0, MoveInstructionType::FREESPACE, "freespace_profile");
  start_instruction.setDescription("Start Instruction");

  // Plan freespace to end
  MoveInstruction plan_f1(wp0, MoveInstructionType::FREESPACE, "freespace_profile");
  plan_f1.setDescription("to_end_plan");

  // Add Instructions to program
  program.appendMoveInstruction(start_instruction);
  program.appendMoveInstruction(plan_f1);

[zzzbbs]

The purpose of my code is to make robot remain fixed, because the start state and the end state are all the same. I try to print out the poses of manipulator waypoints in tesseract_common::Toolpath toToolpath(const CompositeInstruction& ci, const tesseract_environment::Environment& env). It print out the result:

[ INFO] [1724234642.467938074]: Publishing maintained environment on '/tesseract_ros_examples/tesseract_published_environment'
wp1Cart WP: xyz=0.58214, -0.44823, 0.68932
wp2Cart WP: xyz=0.58214, -0.37463, 0.68932
Program: Composite Instruction, Description: Tesseract Composite Instruction
Program: {
Program:   Move Instruction, Move Type: 1, State WP: Pos=     0      0      0      0 -0.842 -1.309  0.227  1.561  0.051  0.754 -1.264
, Description: Start Instruction
Program:   Move Instruction, Move Type: 1, State WP: Pos=     0      0      0      0 -0.842 -1.309  0.227  1.561  0.051  0.754 -1.264
, Description: to_end_plan
Program: }
Info:    basic cartesian plan example
[ERROR] [1724234642.493298260]: Hit Enter to solve for trajectory.

Info:    1 JointVelTermInfo coeffs given. Applying to all 11 joints
Info:    1 JointVelTermInfo targets given. Applying to all 11 joints
Info:    TrajOptMotionPlannerTask motion planning failed (Failed to find valid solution: OPT_PENALTY_ITERATION_LIMIT) for process input: Tesseract Composite Instruction
Info:    Planning took 3.776677 seconds.
[ERROR] [1724234651.444812327]: Hit enter key to continue!

 0.581964 -0.458754  0.689605
 0.53911 -0.48073 0.762438
0.539107 -0.48073 0.762443
0.539106 -0.48073 0.762445
0.539105 -0.48073 0.762446
0.539105 -0.48073 0.762446
0.539105 -0.48073 0.762446
0.539106 -0.48073 0.762445
0.539107 -0.48073 0.762443
 0.53911 -0.48073 0.762438
 0.581964 -0.458754  0.689605

[Levi-Armstrong]

I am confused as to why you would want to send a motion planning request which requires no motion planning because the start and goal are the same? More specific to the additional states; this is the intended behavior for the pipeline you are using. This is controlled by the simple planner and the profiles assigned. The intermediate states are required for motion planning because they represent the path between two state. You can change the profiles configuration for the simple planner to get a different number of intermediate state depending on your motion planning problem.

[zzzbbs]

I am confused as to why you would want to send a motion planning request which requires no motion planning because the start and goal are the same? More specific to the additional states; this is the intended behavior for the pipeline you are using. This is controlled by the simple planner and the profiles assigned. The intermediate states are required for motion planning because they represent the path between two state. You can change the profiles configuration for the simple planner to get a different number of intermediate state depending on your motion planning problem.

Thank you very much for your reply! Sorry for the question because I'm currently not familiar to this package. I reduced to the same start and end state for another confused problem. I make two intermediate cartesian states for motion planning, but only one of them is converged. Thus, I'm trying to understand the changes in the intermediate states.
I thought maybe there is some interpolation process for the input of motion planning, but now I figure out it is the intermediate states of TrajOpt.
I‘m currently using KDLInvKinChainLMA, is it due to the fact that the target pose is not reachable with this solver?