trajopt end_pose displaces 40cm when executed on UR5 directly along x and y-axis

[lakshmip001]

When I executed trajopt optimization on URSim the robot moves with a displacement of 10cm and 7cm along x-axis and y-axis? I am defining pose as a constraint itself but still it displaces by 40cm. Constraints are given as follows

void traj_prob::addLinearMotion(trajopt::ProblemConstructionInfo& pci, Isometry3d start_pose, Isometry3d end_pose, int num_steps, int first_time_step) { 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();

for(int i=0; i<num_steps; i++)
{
    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);
}

}

TrajOptProbPtr traj_prob::generatePickProblem(Isometry3d& approach_pose, Isometry3d& final_pose, int steps_perphase) { 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, 2.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, 2.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>(6, 0.0);
  jv->coeffs = std::vector<double>(6, 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);

  std::shared_ptr<CartVelTermInfo> cv(new CartVelTermInfo);
  cv->term_type = TT_COST;
  cv->first_step = 0;
  cv->last_step = pci.basic_info.n_steps-1;
  cv->max_displacement = 0.05;
  cv->link = "tool0";

  pci.cost_infos.push_back(cv);

  // ================= Path waypoints =======================
  this->addLinearMotion(pci, approach_pose, final_pose, steps_per_phase, steps_per_phase);

  //  this->addTotalTimeCost(pci, 50.0);

TrajOptProbPtr result = ConstructProblem(pci);
return result;

} Kindly please suggest me if I am doing something wrong