Trajopt trajectory optimization

[lakshmip001]

I gave start and end point to addlinearmotion(approach_pose, final_pose, num_steps) but my robot arm moves to a different position as shown below when compare the joint_values obtained to the pose reached by the manipulator are same but my end_pose is different . So I dont know how I can make it reach to the specified end_pose. The values are different.

target_pose.position.x = 0.477;
target_pose.position.y = -0.354;
target_pose.position.z = 0.11;
target_pose.orientation.w = -0.2821;
target_pose.orientation.x = 0.655;
target_pose.orientation.y = -0.2819;
target_pose.orientation.z = -0.6415;

The robot should go this target_pose. But it reaches a point as below.
pose.position.x = -0.590;
pose.position.y = 0.399;
pose.position.z = 0.108

I tried using moveit it reaches exact position.

Is this because optimization assumes a box size for converging the point ?

[mpowelson]

To be clear, addlinearmotion is not a function in this repository. It is a function in the example I send you a while ago. That example was/is still in development, so I can't even guarantee the version I have is the same as what I sent you.

For this issue and any others, it is always useful to include a minimum repeatable example if possible. In this case, a small piece of code that shows how you are setting up your problem and then solving it without any of your other non-trajopt things in it. Many times in making those examples you will find the issue yourself. We certainly welcome questions, and we are excited that you are using TrajOpt. However, it is impossible for us to help with issues like this that basically just say "it doesn't work".

To answer your last point, you should be able to get much closer than that with the default box size (also, box size is not in cartesian space)

[lakshmip001]

I understand your point, Actually I got the forward kinematics of two points in a straight line using ros moveit with respect to a frame. Then costs are set as shown below:

trajopt::ProblemConstructionInfo pci(env_);
  // Add kinematics
  pci.kin = kin_;

  /* Fill Code: Define the basic info
       . Set the pci number of steps
       . Set the start_fixed to false
       . Set the manipulator name (see class members)
       . Set dt lower limit
       . Set dt upper limit
  */
  /* ========  ENTER CODE HERE ======== */
  pci.basic_info.n_steps = steps_per_phase * 2;
  pci.basic_info.manip = manipulator_;
  pci.basic_info.dt_lower_lim = 0.2;
  pci.basic_info.dt_upper_lim = .5;
  pci.basic_info.start_fixed = true;
  pci.basic_info.use_time = false;

  //---------------------------------------------------------
  // ---------------- Fill Init Info ------------------------
  //---------------------------------------------------------

  // To use JOINT_INTERPOLATED - a linear interpolation of joint values
  //  pci.init_info.type = InitInfo::JOINT_INTERPOLATED;
  //  pci.init_info.data = numericalIK(final_pose);  // Note, take the last value off if using time (just want jnt
  //  values)

  // To use STATIONARY - all jnts initialized to the starting value
  pci.init_info.type = InitInfo::STATIONARY;
  pci.init_info.dt = 0.5;

  //---------------------------------------------------------
  // ---------------- Fill Term Infos -----------------------
  //---------------------------------------------------------

  // ================= Collision cost =======================
  std::shared_ptr<CollisionTermInfo> collision(new CollisionTermInfo);
  /* Fill Code:
       . Define the cost name
       . Define the term type (This is a cost)
       . Define this cost as continuous
       . Define the first time step
       . Define the last time step
       . Set the cost gap to be 1
       . Define the penalty type as sco::squared
  */
  /* ========  ENTER CODE HERE ======== */
  collision->name = "collision";
  collision->term_type = TT_COST;
  collision->continuous = true;
  collision->first_step = 0;
  collision->last_step = pci.basic_info.n_steps - 1;
  collision->gap = 1;
  collision->info = createSafetyMarginDataVector(pci.basic_info.n_steps, 0.025, 40);

  pci.cost_infos.push_back(collision);

  // ================= Velocity cost =======================
  std::shared_ptr<JointVelTermInfo> jv(new JointVelTermInfo);

  // Taken from iiwa documentation (radians/s) and scaled by 0.8
  std::vector<double> vel_lower_lim{ 1.71*-0.8, 1.71*-0.8, 1.75*-0.8, 2.27*-0.8, 2.44*-0.8, 3.14*-0.8, 3.14*-0.8 };
  std::vector<double> vel_upper_lim{ 1.71*0.8, 1.71*0.8, 1.75*0.8, 2.27*0.8, 2.44*0.8, 3.14*0.8, 3.14*0.8 };

  /* Fill Code:
       . Define the term time (This is a cost)
       . Define the first time step
       . Define the last time step
       . Define the term name
  */
  /* ========  ENTER CODE HERE ======== */
  jv->targets = std::vector<double>(7, 0.0);
  jv->coeffs = std::vector<double>(7, 5.0);
//  jv->lower_tols = vel_lower_lim;
//  jv->upper_tols = vel_upper_lim;
  jv->term_type = TT_COST;
  jv->first_step = 0;
  jv->last_step = pci.basic_info.n_steps - 1;
  jv->name = "joint_velocity_cost";

  pci.cost_infos.push_back(jv);

Then later displacement for each step is obtained as shown below:

 Vector3d xyz_delta = (end_pose.translation() - start_pose.translation()) / (num_steps - 1);

  Quaterniond approach_rotation(start_pose.linear());
  Matrix3d rotation_diff = (start_pose.linear().inverse() * end_pose.linear());
  AngleAxisd aa_rotation_diff(rotation_diff);
  double angle_delta = aa_rotation_diff.angle() / (num_steps - 1);
  Vector3d delta_axis = aa_rotation_diff.axis();

  // Create a series of pose constraints for linear pick motion
  for (int i = 0; i < num_steps; i++)
  {
    /* Fill Code:
         . Create a new shared_ptr<StaticPoseCostInfo>
         . Define the term type (This is a constraint)
         . Set the link constrained as the end effector (see class members)
         . Set the correct time step for this pose
         . Set the pose xyz translation
    */
    /* ========  ENTER CODE HERE ======== */
    std::shared_ptr<CartPoseTermInfo> pose_constraint = std::shared_ptr<CartPoseTermInfo>(new CartPoseTermInfo);
    pose_constraint->term_type = TT_CNT;
    pose_constraint->link = ee_link_;
    pose_constraint->timestep = i + first_time_step;
    pose_constraint->xyz = start_pose.translation() + xyz_delta * i;

    Quaterniond rotation_delta(cos(0.5 * angle_delta * i),
                               delta_axis.x() * sin(0.5 * angle_delta * i),
                               delta_axis.y() * sin(0.5 * angle_delta * i),
                               delta_axis.z() * sin(0.5 * angle_delta * i));
    Quaterniond rotation = rotation_delta * approach_rotation;

    /* Fill Code:
         . Set the pose rotation
         . Set pos_coeffs to all 10s
         . Set rot_coeffs to all 10s
         . Define the pose name as pose_[timestep]
         . pushback the constraint to cnt_infos
    */
    /* ========  ENTER CODE HERE ======== */
    pose_constraint->wxyz = Vector4d(rotation.w(), rotation.x(), rotation.y(), rotation.z());
    pose_constraint->pos_coeffs = Vector3d(10.0, 10.0, 10.0);
    pose_constraint->rot_coeffs = Vector3d(10.0, 10.0, 10.0);
    pose_constraint->name = "pose_" + std::to_string(i + first_time_step);
    pci.cnt_infos.push_back(pose_constraint);

Then later result is obtained after trajectory optimization as follows:

// Set the parameters
    tesseract::tesseract_planning::TrajOptPlannerConfig config(pick_prob);
    config.params.max_iter = 500;

    // Create Plot Callback
    if (plotting_cb)
    {
      tesseract::tesseract_ros::ROSBasicPlottingPtr plotter_ptr(new tesseract::tesseract_ros::ROSBasicPlotting(env));
      config.callbacks.push_back(PlotCallback(*pick_prob, plotter_ptr));
    }
    // Create file write callback discarding any of the file's current contents
    std::shared_ptr<std::ofstream> stream_ptr(new std::ofstream);
    if (file_write_cb)
    {
      std::string path = ros::package::getPath("pick_and_place") + "/file_output_pick.csv";
      stream_ptr->open(path, std::ofstream::out | std::ofstream::trunc);
      config.callbacks.push_back(trajopt::WriteCallback(stream_ptr, pick_prob));
    }

    // Solve problem
    planner.solve(planning_response, config);

I didn't understand how does plotting_cb and file_write_cb can be used for tesseract. Kindly please help me to understand.

planning_response is given to /follow_joint_trajectory using ROS moveit IPTP.

I checked with the end point joint values of the joint trajectory obtained by trajopt and my robot is at almost that position. So I feel something is wrong while optimizing trajectory(like 50% of trajectory is optimized or something). Kindly please let me know If I am wrong at any point.