Ask Your Question
0

How to get velocity, acceleration of joints in TrajOpt trajectory?

asked 2019-03-17 05:20:14 -0500

DieterWuytens gravatar image

updated 2019-03-17 05:23:41 -0500

Hi, i'm working with TrajOpt and working with the basic_cartesian_path robot example file.

Planning with the example package is working (tesseract + trajopt_ros) but i'm asking if it is possible to see the trajectory (so joint values, speed and acceleration) After launching the launch file and rviz i execude the basic_cartesian_path.cpp file. And get the following: (i have changed the code to work with my 8DOF robot)

            |   oldexact |    dapprox |     dexact |      ratio
      COSTS | -------------------------------------------------
  joint_vel |  3.471e+00 |  1.179e-02 |  1.179e-02 |  1.000e+00
  joint_acc |  2.558e+00 |  1.699e-02 |  1.699e-02 |  1.000e+00
 joint_jerk |  1.072e+01 |  7.395e+00 |  6.327e-02 |  8.555e-03
collision_0 |  0.000e+00 | -1.631e-12 |  0.000e+00 |   ------  
collision_1 |  0.000e+00 | -1.631e-12 |  0.000e+00 |   ------  
collision_2 |  0.000e+00 | -1.631e-12 |  0.000e+00 |   ------

This is just a small part of the entire displayed values in the terminal. I have added the following lines in de cpp file to get the trajectory after optimaization:

> TrajArray output = getTraj(opt.x(), prob->GetVars());   
> ROS_INFO_STREAM("\n" << output );

It gives me the joint value (degrees) of each joints. (8DOF robot)

     3.63784 -4.49758e-05    0.0451132    0.203       1.0405   0.00737547 -4.46337e-05    -0.503622          
     3.65105 -4.46174e-05    0.0603891    0.706067  0.527483   0.00755458 -4.42784e-05    -0.517012          
     3.66406 -4.42575e-05    0.0604344    0.471696      0.76724   0.00773493 -4.39215e-05    -0.5302
     3.67687 -4.38968e-05    0.283819      0.44633     0.574757   0.00791593 -4.35638e-05    -0.54318
     3.68947 -4.35354e-05    0.183435     0.712521     0.414586   0.00809789 -4.32054e-05    -0.55597
     3.70188 -4.31735e-05    0            0.777079     0.539197   0.00828025 -4.28467e-05      -0.5

But where do I get the velocity, acceleration of each joint? There is also no indication of the time between these points. Or am I missing something here?

I'm using kinetic.

edit retag flag offensive close merge delete

1 Answer

Sort by ยป oldest newest most voted
0

answered 2019-04-30 02:54:51 -0500

Lucky251293 gravatar image

updated 2019-04-30 02:56:15 -0500

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

' ' '

edit flag offensive delete link more

Comments

+1 for sharing your approach, but please note that with the code you show, you are asking the MoveIt IPTP to reparameterise the trajectory based on the timestamps coming from the output of Trajopt. That is different from getting velocities and accelerations from the trajopt output.

gvdhoorn gravatar image gvdhoorn  ( 2019-04-30 03:04:06 -0500 )edit

Hi, thanks for your approach. I use "getTraj(opt.x(), prob->GetVars())" To get the TrajArray I don't use the "planning_response" and "mgi" i just have:

TrajOptProbPtr prob;

prob = cppMethod();

sco::BasicTrustRegionSQP opt(prob);

opt.optimize();

DieterWuytens gravatar image DieterWuytens  ( 2019-04-30 14:55:10 -0500 )edit

Your Answer

Please start posting anonymously - your entry will be published after you log in or create a new account.

Add Answer

Question Tools

2 followers

Stats

Asked: 2019-03-17 05:20:14 -0500

Seen: 363 times

Last updated: Apr 30 '19