cartesian velocity relation with IPTP vel and acc scaling , trajopt JointVelTermInfo() and CartVelTermInfo()

[lakshmip001]

I obtained trajectory by trajopt, then I assign the timestamps and then obtain joint velocities and joint accelerations using IPTP of moveit as shown below . I need to relate cartesian velocity obtained with trajopt parameters and moveit parameters and time stamps. So I can change parameter required for getting the desired velocity.

Parameters defined in trajopt 👍

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);

Parameters defined in moveit:

double max_velocity_scale = 0.4, max_acceleration_scale = 1;

//timestamp assignment
ros::Duration t1 = ros::Duration(0.25);
        traj_msg = trajArrayToJointTrajectoryMsg(planning_response.joint_names, planning_response.trajectory, t1);
        std::cout<<"trajectory message"<<traj_msg;
        TrajArray result = planning_response.trajectory;

        traj_msg3.joint_names = env->getJointNames();
        for (int i = 0; i < result.rows(); ++i)
        {
          trajectory_msgs::JointTrajectoryPoint jtp;
          for (int j = 0; j < result.cols(); ++j)
          {
            jtp.positions.push_back(result(i, j));
          }
          jtp.time_from_start = t1 * i;
          traj_msg3.points.push_back(jtp);
        }
//joint velocities and accelerations:
std::cout << "calculating vel and acc using iptp"<<std::endl;
        
      robot_trajectory::RobotTrajectory rt(mgi->getRobotModel(), "manipulator");
      std::cout <<"robot trajectory is created"<<std::endl;
      rt.setRobotTrajectoryMsg(*mgi->getCurrentState(), traj_msg3);
        // Second get a RobotTrajectory from trajectory
         std::cout << "robot trajectory message"<<std::endl;

        // Thrid create a IterativeParabolicTimeParameterization object
        trajectory_processing::IterativeParabolicTimeParameterization iptp;
        std::cout << "iptp" <<std::endl;
        // Fourth compute computeTimeStamps
        bool success = iptp.computeTimeStamps(rt, max_velocity_scale,   max_acceleration_scale);
        rt.getRobotTrajectoryMsg(trajectory);

Obtained Cartesian Velocity profile with the above parameters:

[mpowelson]

Maybe this doesn't answer your question, but the limitation in TrajOpt when not using time parameterization is that a unit time step is always assumed. ie, instead of v = (x2-x1)/dt we calculate it as v= (x2-x1). This is due to the way the quadratic programs are defined. In practice, when the velocity target is 0, this just tends to make the trajectories smooth rather than jumping back and forth between configurations, etc.

It sounds like you are trying to get a specific velocity profile. At the moment I would recommend that you optimize the kinematics with trajopt and the timesteps with something else (like you are doing). I think iterating between IPTP and Trajopt to get the right profile is the wrong way to go about it. I would optimize the kinematics with TrajOpt and then do whatever you need to with the timesteps to make the velocities work out. If you're wanting a special profile that the off the shelf IPTP doesn't give you, you may need to make something custom. Regardless, that's in a different repo, and I don't know enough about that code to help you. I realize it's not a perfect answer, but it's about the best we have right now.

I suppose another option would just be to assume a fixed time step and optimize with a really dense trajectory. Then set your velocity, acceleration, jerk limits to be whatever they should be when scaled to a unit time step. Then TrajOpt should give you what you want directly. Just keep in mind that we are taking numeric derivatives, so you'll need pretty dense trajectories for things like acceleration and jerk to be terribly meaningful