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

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

DieterWuytens
15 ●13 ●15 ●18

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 reopen merge delete

Closed for the following reason question is not relevant or outdated by tfoote
close date 2022-10-12 04:52:10.794275

add a comment

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

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 ( 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 ( 2019-04-30 14:55:10 -0500 )edit

add a comment

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

Closed for the following reason question is not relevant or outdated by tfoote
close date 2022-10-12 04:52:10.794275

1 Answer

Comments

Question Tools

Stats

Related questions

How to get velocity, acceleration of joints in TrajOpt trajectory? [closed] edit

Closed for the following reason question is not relevant or outdated by tfoote close date 2022-10-12 04:52:10.794275

1 Answer

Comments

Question Tools

Stats

Related questions

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

Closed for the following reason question is not relevant or outdated by tfoote
close date 2022-10-12 04:52:10.794275