Ask Your Question
0

Trajectory Splicing Error

asked 2018-08-22 12:14:15 -0500

shreyasgan gravatar image

updated 2018-08-23 17:47:31 -0500

jayess gravatar image

Hello,

I am trying to send my Staubli TX90 robot arm a Joint Trajectory command which publishes to /joint_path_command, which the motion_streaming_interface node subscribes to. My code complies and build correctly, but when I run the node, there is an error saying "Trajectory splicing not yet implemented, stopping current motion." I looked online and noticed that:

// read current state value (should be atomic)
int state = this->state_;

ROS_DEBUG("Current state is: %d", state);

if (TransferStates::IDLE != state)
{

if (msg->points.empty())

ROS_INFO("Empty trajectory received, canceling current trajectory");

else

ROS_ERROR("Trajectory splicing not yet implemented, stopping current motion.");

     this->mutex_.lock();

trajectoryStop();

     this->mutex_.unlock();

return;

This is my code for the node that I am running to communicate with the robot arm and subscribe to /joint_states and publish to /joint_path_command:

using namespace std;

sensor_msgs::JointState JS;

void jointStateCallback(const sensor_msgs::JointState msg) { JS = msg; }

int main(int argc, char** argv) { 
  ros::init(argc, argv, "mover_node");

  ros::NodeHandle n;
  ros::AsyncSpinner spinner(1);
  spinner.start();

  bool success;

  ros::Publisher joint_pub = n.advertise<trajectory_msgs::jointtrajectory>("/joint_path_command", 1);
  ros::Subscriber joint_sub = n.subscribe<sensor_msgs::jointstate>("/joint_states", 1, jointStateCallback);
  ros::Rate rate(10.0);

  trajectory_msgs::JointTrajectory JT;

  JT.joint_names.clear();
  JT.joint_names.push_back("joint_1");
  JT.joint_names.push_back("joint_2");
  JT.joint_names.push_back("joint_3");
  JT.joint_names.push_back("joint_4");
  JT.joint_names.push_back("joint_5");
  JT.joint_names.push_back("joint_6");

  JT.points.resize(2);
  JT.points[0].positions.resize(JT.joint_names.size());
  JT.points[1].positions.resize(JT.joint_names.size());

  while(ros::ok()) {

    if(JS.position.size() != 0)
    {
      cout << JT << endl;

      JT.points[0].time_from_start = ros::Duration(0.0001);
      JT.points[0].positions = JS.position;

      JT.points[1].positions[0] = 0.0;
      JT.points[1].positions[1] = 0.0;
      JT.points[1].positions[2] = 0.0;
      JT.points[1].positions[3] = 0.0;
      JT.points[1].positions[4] = 0.0;
      JT.points[1].positions[5] = 0.0;
      JT.points[1].time_from_start = ros::Duration(1);

      joint_pub.publish(JT);
    }
    rate.sleep();
  } 
  ros::shutdown();

  return 0;
}

The values of the JT object are:

header:

seq: 0

stamp: 0.000000000

frame_id:

joint_names[]

joint_names[0]: joint_1

joint_names[1]: joint_2

joint_names[2]: joint_3

joint_names[3]: joint_4

joint_names[4]: joint_5

joint_names[5]: joint_6

points[]

points[0]:

positions[]

  positions[0]: 0.883191

  positions[1]: -0.00796466

  positions[2]: -0.0596276

  positions[3]: 3.05678

  positions[4]: -0.00680966

  positions[5]: 0.0617737

velocities[]

accelerations[]

effort[]

time_from_start: 0.000100000

points[1]:

positions[]

  positions[0]: 0

  positions[1]: 0

  positions[2]: 0

  positions[3]: 0

  positions[4]: 0

  positions[5]: 0

velocities[]

accelerations[]

effort[]

time_from_start: 1.000000000
edit retag flag offensive close merge delete

1 Answer

Sort by ยป oldest newest most voted
0

answered 2018-08-22 13:53:53 -0500

gvdhoorn gravatar image

updated 2018-08-24 11:14:16 -0500

but when I run the node, there is an error saying "Trajectory splicing not yet implemented, stopping current motion

the industrial_robot_client nodes don't support trajectory splicing (blending or stitching of subsequent trajectories).

You'll have to make sure that the previous trajectory has finished before sending a new one.

The /joint_path_command topic accepts trajectories, not single points, so sending it new trajectories at 10 Hz like the code does that you show will not work.


so I am just trying to send a simple command to my TX90 robot arm, which has ROS integration. My final goal is to send a set of predefined waypoints to a cartesian planner, and publish that trajectory to the TX90 arm (end effector control).

Then create a single trajectory with as many points as you need (but know that dense trajectories typically result in slower motion). Send that complete trajectory in one msg to the /joint_path_command topic.

Or make use of the action server that is started by the driver launch file. That would accept FollowJointTrajectory actions, which are a little friendlier in use than a raw topic. For one thing, they would allow you to wait for trajectory execution to complete, and notify you when that happens.

edit flag offensive delete link more

Comments

Okay, so I would have to make the JointTrajectory points in my JT object have more than one set of points, or add a velocity parameter to the JT message I publish to motion_streaming_interface? Also is it necessary to make the first set of joint positions in the trajectory equal to th current state?

shreyasgan gravatar imageshreyasgan ( 2018-08-23 15:11:05 -0500 )edit

I have now updated my code to send a Joint Trajectory message with two sets of points, yet I am still getting the same Trajectory splicing not implemented Error. I have also added the values of the JT object. Any help is greatly appreciated.

shreyasgan gravatar imageshreyasgan ( 2018-08-23 16:16:39 -0500 )edit
1

You're still sending trajectories at 10Hz to the driver. You have to check whether the previous trajectory has finished executing. If it hasn't and you set a new one, you'll get that error.

gvdhoorn gravatar imagegvdhoorn ( 2018-08-24 02:06:32 -0500 )edit

Perhaps it would be good if you can provide a little more information on what it is that you're trying to achieve. Right now we're focusing on a very specific detail of an approach that might not be appropriate for what you're trying to do.

gvdhoorn gravatar imagegvdhoorn ( 2018-08-24 02:07:28 -0500 )edit

Alright, so I am just trying to send a simple command to my TX90 robot arm, which has ROS integration. My final goal is to send a set of predefined waypoints to a cartesian planner, and publish that trajectory to the TX90 arm (end effector control).

shreyasgan gravatar imageshreyasgan ( 2018-08-24 11:09:13 -0500 )edit

Thank you! The code works now that I have removed the rate(10.0) line. I was wondering, how would I send an end effector trajectory (computed by a cartesian planner) to one of the topics that industrial_robot_client subscribes to, when it only subscribes to topics that use Joint Trajectory messages

shreyasgan gravatar imageshreyasgan ( 2018-08-24 12:54:11 -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: 2018-08-22 12:14:15 -0500

Seen: 88 times

Last updated: Aug 24 '18