Use toppra with ROS Moveit

[rennico]

Hello,
I'm trying to use toppra with Moveit to plan a less jerky trajectory for a ur5 robot.
I started with the python API casuse and it was very easy to get my hands on it. Unfortunately when I tried to use it with ROS, I got problems with the python version. My ROS version is Melodic which supports only python 2.7. But I built my toppra libraries on python 3.7.4 as suggested ... thus I turned to CPP API, I have difficulties with C++, I read all the tests files and the issues, i barely understand ... this is the code I wrote refers to the test_issue_198.cpp. I got segmentation faults and core dumped every time.

Do you have other short easier C++ API tutorial please ? My goal is try to re-time path with velocity and acceleration bounds.
Thank you very much.

[rennico]

`
#include<ros/ros.h>
#include<moveit/move_group_interface/move_group_interface.h>
#include<moveit/robot_trajectory/robot_trajectory.h>
#include
#include
#include "Eigen/Dense"
#include <gtest/gtest.h>

#include <toppra/algorithm/toppra.hpp>
#include <toppra/constraint/linear_joint_acceleration.hpp>
#include <toppra/constraint/linear_joint_velocity.hpp>
#include <toppra/geometric_path/piecewise_poly_path.hpp>
#include <toppra/parametrizer/const_accel.hpp>
#include <toppra/parametrizer/spline.hpp>
#include <toppra/toppra.hpp>

using namespace std;
using namespace Eigen;

void write_trajectory(moveit::planning_interface::MoveGroupInterface::Plan &plan)
{
int n_joints = plan.trajectory_.joint_trajectory.joint_names.size();

string filePath="/home/ren/trajectory/traj.txt";
ofstream fout(filePath);
if(!fout)
{
cout << "Can not open output file"<<endl;
}
for(int i=0; i<plan.trajectory_.joint_trajectory.points.size(); i++)
{
    for(int j=0; j<n_joints; j++)
{
        fout << plan.trajectory_.joint_trajectory.points[i].velocities[j] << " "<< plan.trajectory_.joint_trajectory.points[i].accelerations[j] << " "<< plan.trajectory_.joint_trajectory.points[i].positions[j] << " ";
}
fout << endl;
}
fout.close();

}

int main(int argc, char **argv)
{
ros::init(argc,argv,"moveit_circle_demo");
ros::AsyncSpinner spinner(1);
spinner.start();

moveit::planning_interface::MoveGroupInterface arm("manipulator");
std::string end_effector_link = arm.getEndEffectorLink();
std::string reference_frame = "base_link";
arm.setPoseReferenceFrame(reference_frame);


arm.allowReplanning(true);


arm.setGoalPositionTolerance(0.001);
arm.setGoalOrientationTolerance(0.01);


arm.setMaxAccelerationScalingFactor(1);
arm.setMaxVelocityScalingFactor(1);


arm.setNamedTarget("home");
arm.move();
sleep(1);
std::cout<< "start" <<std::endl;
// set pose target
geometry_msgs::Pose target_pose;
target_pose.orientation.x = 0;
target_pose.orientation.y = 1;
target_pose.orientation.z = 0;
target_pose.orientation.w = 0;

std::cout<< "Target x y z" <<std::endl;

target_pose.position.x = 0.28;
target_pose.position.y = 0.31;
target_pose.position.z = 0.37;

std::cout<< "circle center :"<< target_pose.position.x  << target_pose.position.y  <<target_pose.position.z << std::endl;
arm.setPoseTarget(target_pose);
arm.move();
sleep(1);


// move back to home pose
arm.setNamedTarget("home");
moveit::planning_interface::MoveGroupInterface::Plan plan;
arm.plan(plan);
//write_trajectory(plan);
//arm.move();
sleep(1);

//-----------------------------------------------------------toppra
const int kNumSamples = plan.trajectory_.joint_trajectory.points.size();
const int kNumJoints =plan.trajectory_.joint_trajectory.joint_names.size() ;

VectorXd q1(kNumSamples);
VectorXd q2(kNumSamples);
VectorXd q3(kNumSamples);
VectorXd q4(kNumSamples);
VectorXd q5(kNumSamples);
VectorXd q6(kNumSamples);

VectorXd qdot1(kNumSamples);
VectorXd qdot2(kNumSamples);
VectorXd qdot3(kNumSamples);
VectorXd qdot4(kNumSamples);
VectorXd qdot5(kNumSamples);
VectorXd qdot6(kNumSamples);

for(int j=0; j<kNumSamples; j++)
{

	  q1(j) = plan.trajectory_.joint_trajectory.points[j].positions[1];
	  qdot1(j) = plan.trajectory_.joint_trajectory.points[j].velocities[1];

	  q2(j) = plan.trajectory_.joint_trajectory.points[j].positions[2];
	  qdot2(j) = plan.trajectory_.joint_trajectory.points[j].velocities[2];

	  q3(j) = plan.trajectory_.joint_trajectory.points[j].positions[3];
	  qdot3(j) = plan.trajectory_.joint_trajectory.points[j].velocities[3];

	  q4(j) = plan.trajectory_.joint_trajectory.points[j].positions[4];
	  qdot4(j) = plan.trajectory_.joint_trajectory.points[j].velocities[4];

	  q5(j) = plan.trajectory_.joint_trajectory.points[j].positions[5];
	  qdot5(j) = plan.trajectory_.joint_trajectory.points[j].velocities[5];

	  q6(j) = plan.trajectory_.joint_trajectory.points[j].positions[6];
	  qdot6(j) = plan.trajectory_.joint_trajectory.points[j].velocities[6];
}

const std::vector q{q1, q2, q3, q4, q5, q6};
const std::vector q_dot{qdot1, qdot2, qdot3, qdot4, qdot5, qdot6 };

VectorXd lower_acceleration_limit(kNumJoints);
lower_acceleration_limit << -4.57, -4.57, -4.57, -4.57, -4.57, -4.57;
VectorXd upper_acceleration_limit(kNumJoints);
upper_acceleration_limit << 4.57, 4.57, 4.57, 4.57, 4.57, 4.57;

VectorXd velocity_limit(kNumJoints);
velocity_limit << 2.2943951023931953,2.2943951023931953, 3.1415926535897931, 3.1415926535897931, 3.1415926535897931, 3.1415926535897931;

// TOPPRA
const double kStartTime = 0.0;
const double kEndTimes = 2;
toppra::Vector times_toppra = toppra::Vector::LinSpaced(kNumSamples, kStartTime, kEndTimes);
std::vector times;
for (size_t i = 0; i < kNumSamples; ++i) times.push_back(times_toppra(i));

toppra::Vectors q_nominal_vectors;
toppra::Vectors q_dot_nominal_vectors;
for (int i = 0; i < kNumSamples; i++) {
q_nominal_vectors.push_back(q[i]);
q_dot_nominal_vectors.push_back(q_dot[i]);
}

toppra::PiecewisePolyPath hermite = toppra::PiecewisePolyPath::constructHermite(q_nominal_vectors, q_dot_nominal_vectors, times);
toppra::GeometricPathPtr path;
path = std::make_sharedtoppra::PiecewisePolyPath(hermite);

toppra::LinearConstraintPtr velocity_constraint =
std::make_sharedtoppra::constraint::LinearJointVelocity(-velocity_limit, velocity_limit);
velocity_constraint->discretizationType(toppra::DiscretizationType::Interpolation);

// set acceleration constraint
toppra::LinearConstraintPtr acceleration_constraint =
std::make_sharedtoppra::constraint::LinearJointAcceleration(lower_acceleration_limit, upper_acceleration_limit);
acceleration_constraint->discretizationType(toppra::DiscretizationType::Interpolation);

// Create linear joint space constraints.
toppra::LinearConstraintPtrs constraints{velocity_constraint, acceleration_constraint};

// set auto gridpoints
int N=0;
// Compute the toppra algorithm.
auto algo = std::make_sharedtoppra::algorithm::TOPPRA(constraints, path);
algo->setN(N);
toppra::ReturnCode return_code =
algo->computePathParametrization(0 /Zero Initial Velocity/, 0 /Zero Final Velocity/);
//ASSERT_EQ(return_code, toppra::ReturnCode::OK);

toppra::ParametrizationData data = algo->getParameterizationData();

//Create constant acceleration parametrizer.
auto gridpoints = data.gridpoints; // Grid-points
cout<<"Gridpoints has " << gridpoints.size() << " points"<<endl;

toppra::Vector vsquared = data.parametrization; // (squared path velocity)
auto spline_path = std::make_sharedtoppra::parametrizer::Spline(path, gridpoints, vsquared);

const toppra::Bound optimized_time_interval = spline_path->pathInterval();
cout<<"Optimized time: " << optimized_time_interval << endl;

toppra::Vector time_breaks_optimized =
toppra::Vector::LinSpaced(5 * kNumSamples, optimized_time_interval(0), optimized_time_interval(1));
toppra::Vectors q_nominal_optimized = spline_path->eval(time_breaks_optimized, 0);
toppra::Vectors q_dot_nominal_optimized = spline_path->eval(time_breaks_optimized, 1);
toppra::Vectors q_ddot_nominal_optimized = spline_path->eval(time_breaks_optimized, 2);

std::vector q_nominal_optimized_mat{};
std::vector q_dot_nominal_optimized_mat{};
std::vector time_breaks_optimized_vec{};

// Now append the rest of the optimized path points generated by toppra.
const double kTolerance = 1.001;
for (size_t i = 0; i < 5 * kNumSamples; ++i) {
const double t = time_breaks_optimized[i];
const toppra::Vector& q_t = q_nominal_optimized[i];
const toppra::Vector& q_dot_t = q_dot_nominal_optimized[i];
const toppra::Vector q_ddot_t = q_ddot_nominal_optimized[i];
/* for (int j = 0; j < kNumJoints; j++) {
ASSERT_LT(q_ddot_t[j], kTolerance * upper_acceleration_limit[j]);
ASSERT_LT(q_dot_t[j], kTolerance * velocity_limit[j]);
ASSERT_GT(q_ddot_t[j], kTolerance * lower_acceleration_limit[j]);
ASSERT_GT(q_dot_t[j], -kTolerance * velocity_limit[j]);
}*/
q_nominal_optimized_mat.push_back(q_t);
q_dot_nominal_optimized_mat.push_back(q_dot_t);
time_breaks_optimized_vec.push_back(t);
}

ros::shutdown();
return 0;
}

`