How to send TrajOpt Trajectory to UR5

[dieterwuytens]

Currently working on a project with a Universal robot UR5.
I want to send a trajopt trajectory to my robot.
In RVIZ i can see that the trajectory is correct on the topic: trajopt/display_tesseract_trajectory

Now i try to send that trajectory to my UR5-robot.

The output of trajopt (positions) is generated with the following line:

TrajArray output = getTraj(opt.x(), prob->GetVars());

With the next piece of code i can generate a jointTrajectory:

// Create action client to send trajectories
    actionlib::SimpleActionClient<control_msgs::FollowJointTrajectoryAction>execution_client("follow_joint_trajectory", true);
    execution_client.waitForServer();

    // Convert TrajArray (Eigen Matrix of joint values) to ROS message
    trajectory_msgs::JointTrajectory traj_msg;
    ros::Duration t(0.25);

    std::vector<std::string> joint_names;
    joint_names.push_back("shoulder_pan_joint");
    joint_names.push_back("shoulder_lift_joint");
    joint_names.push_back("elbow_joint");
    joint_names.push_back("wrist_1_joint");
    joint_names.push_back("wrist_2_joint");
    joint_names.push_back("wrist_3_joint");

    traj_msg = trajArrayToJointTrajectoryMsg(joint_names, output, false, t);

    // Create action message
    control_msgs::FollowJointTrajectoryGoal trajectory_action;
    trajectory_action.trajectory = traj_msg;
    //        trajectory_action.trajectory.header.frame_id="world";
    //        trajectory_action.trajectory.header.stamp = ros::Time(0);
    //        trajectory_action.goal_time_tolerance = ros::Duration(1.0);
    // May need to update other tolerances as well.

    // Send to hardware
    execution_client.sendGoal(trajectory_action);
    execution_client.waitForResult(ros::Duration(20.0));

    if (execution_client.getState() == actionlib::SimpleClientGoalState::SUCCEEDED)
    {
      std::cout << "Action succeeded! \n";
    }
    else
    {
      std::cout << "Action failed \n";
    }

The function "trajArrayToJointTrajectoryMsg" is the following:

trajectory_msgs::JointTrajectory trajArrayToJointTrajectoryMsg(std::vector<std::string> joint_names,
                                                               tesseract::TrajArray traj_array,
                                                               bool use_time,
                                                               ros::Duration time_increment)
{
  // Create the joint trajectory
  trajectory_msgs::JointTrajectory traj_msg;
  traj_msg.header.stamp = ros::Time::now();
  traj_msg.header.frame_id = "0";
  traj_msg.joint_names = joint_names;

  tesseract::TrajArray pos_mat;
  tesseract::TrajArray time_mat;
  if (use_time)
  {
    // Seperate out the time data in the last column from the joint position data
    pos_mat = traj_array.leftCols(traj_array.cols());
    time_mat = traj_array.rightCols(1);
  }
  else
  {
    pos_mat = traj_array;
  }

  ros::Duration time_from_start(0);
  for (int ind = 0; ind < traj_array.rows(); ind++)
  {
    // Create trajectory point
    trajectory_msgs::JointTrajectoryPoint traj_point;

    // Set the position for this time step
    auto mat = pos_mat.row(ind);
    std::vector<double> vec(mat.data(), mat.data() + mat.rows() * mat.cols());
    traj_point.positions = vec;

    // Add the current dt to the time_from_start
    if (use_time)
    {
      time_from_start += ros::Duration(time_mat(ind, time_mat.cols() - 1));
    }
    else
    {
      time_from_start += time_increment;
    }
    traj_point.time_from_start = time_from_start;

    traj_msg.points.push_back(traj_point);
  }
  return traj_msg;
}

But if i send the trajectory i receive the error: "received goal without velocities"
I guess because the TrajArray only contains joint-positions right?

I tried to add velocity's to the trajectory using IterativeParabolicTimeParameterization suggested by gvdhoorn in this link.

The code to convert that is the following:

// First to create a RobotTrajectory object
    moveit::planning_interface::MoveGroup group("manipulator");

    group.setPlanningTime(20.0);
    robot_trajectory::RobotTrajectory rt(group.getCurrentState()->getRobotModel(), "manipulator");
    std::string eef_link = group.getEndEffectorLink();
    std::string eef = group.getEndEffector();
    std::string base_link = group.getPoseReferenceFrame();
    group.setStartStateToCurrentState();
    group.setPoseReferenceFrame(base_link);
    group.setEndEffectorLink(eef_link);

    // Second get a RobotTrajectory from trajectory
    rt.setRobotTrajectoryMsg(*group.getCurrentState(), traj_msg);
  
    // Thrid create a IterativeParabolicTimeParameterization object
    trajectory_processing::IterativeParabolicTimeParameterization iptp;

    // Fourth compute computeTimeStamps
    success = iptp.computeTimeStamps(rt);
    ROS_INFO("Computed time stamp %s",success?"SUCCEDED":"FAILED");

    // Get RobotTrajectory_msg from RobotTrajectory
    moveit_msgs::RobotTrajectory new_traj;
    rt.getRobotTrajectoryMsg(new_traj);

I'm not sure if that is correct ? It is not possible to send a RobotTrajectory to the FollowJointTrajectoryGoal of control_msgs ..?
I was wondering where do i get the velocity's or how do i add them to the trajopt result. I'm new to this so maybe i missed something?

Any help is appreciated. Thanks.

[Levi-Armstrong]

The RobotTrajectoryMsg contains a JointTrajectoryMsg that you can send to the robot.

[dieterwuytens]

I tried obtaining the JointTrajectoryMsg from the RobotTrajectory and the error of "no vellocity" is gone. But the UR5 doesn't execute the trajectory ... and i receive no errors.

    // Get RobotTrajectory_msg from RobotTrajectory
    moveit_msgs::RobotTrajectory new_traj;
    rt.getRobotTrajectoryMsg(new_traj);
    
    // Create action message
    control_msgs::FollowJointTrajectoryGoal trajectory_action;
    trajectory_action.trajectory = new_traj.joint_trajectory;

    // Send to hardware
    execution_client.sendGoal(trajectory_action);
    execution_client.waitForResult(ros::Duration(20.0));

Any suggestions what i'm doing wrong here?

[Levi-Armstrong]

Reopen if needed.