Revision history [back]

Hi, you can get joint velocities, joint accelerations using ROS MoveIt after getting trajectory from TrajOpt as shown below:

''' trajectory_msgs::JointTrajectory traj_msg3,traj_msg; moveit_msgs::RobotTrajectory trajectory; //test_pub.publish(traj_msg); ros::Duration t1 = ros::Duration(1.0); // traj_msg = trajArrayToJointTrajectoryMsg(planning_response.joint_names, planning_response.trajectory, t1);

    TrajArray result = planning_response.trajectory;
    std::cout << "result after trajopt" << result << std::endl;

    traj_msg3.joint_names = planning_response.joint_names;
    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);
    }
    std::cout<<"trajectory message"<<traj_msg3;
    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);

    std::cout<<"vel & acc add"<< trajectory << std::endl;
    actionlib::SimpleActionClient<control_msgs::FollowJointTrajectoryAction> ac ("/follow_joint_trajectory", true);

' ' '

Hi, you can get joint velocities, joint accelerations using ROS MoveIt after getting trajectory from TrajOpt as shown below:

''' ' ' ' trajectory_msgs::JointTrajectory traj_msg3,traj_msg; moveit_msgs::RobotTrajectory trajectory; //test_pub.publish(traj_msg); ros::Duration t1 = ros::Duration(1.0); // traj_msg = trajArrayToJointTrajectoryMsg(planning_response.joint_names, planning_response.trajectory, t1);

    TrajArray result = planning_response.trajectory;
    std::cout << "result after trajopt" << result << std::endl;

    traj_msg3.joint_names = planning_response.joint_names;
    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);
    }
    std::cout<<"trajectory message"<<traj_msg3;
    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);

    std::cout<<"vel & acc add"<< trajectory << std::endl;
    actionlib::SimpleActionClient<control_msgs::FollowJointTrajectoryAction> ac ("/follow_joint_trajectory", true);

' ' '

Hi, you can get joint velocities, joint accelerations using ROS MoveIt after getting trajectory from TrajOpt as shown below:

' ' ' '

   trajectory_msgs::JointTrajectory traj_msg3,traj_msg;
     moveit_msgs::RobotTrajectory trajectory;
     //test_pub.publish(traj_msg);
     ros::Duration t1 = ros::Duration(1.0);
    // traj_msg = trajArrayToJointTrajectoryMsg(planning_response.joint_names, planning_response.trajectory, t1);
 
t1);

    TrajArray result = planning_response.trajectory;
    std::cout << "result after trajopt" << result << std::endl;

    traj_msg3.joint_names = planning_response.joint_names;
    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);
    }
    std::cout<<"trajectory message"<<traj_msg3;
    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);

    std::cout<<"vel & acc add"<< trajectory << std::endl;
    actionlib::SimpleActionClient<control_msgs::FollowJointTrajectoryAction> ac ("/follow_joint_trajectory", true);
 
' ' '