0
$\begingroup$

Rosanswers logo

Hey,

I am trying to publish multiple trajectory points of type trajectory_msgs::JointTrajectoryPoint on /arm_manipulator_controller/command topic of type trajectory_msgs::JointTrajectoryPoint.

When I am running the node, my robotic arm is performing only first trajectory . The other two trajectories are performed when I am pressing ctrl+c command. Output of rostopic echo /arm_manipulator_command suggesting all three trajectory points are getting published correctly.

Is it happening because of inconsistent time_from_start parameter of trajectory_msgs::JointTrajectoryPoint ?

Output of rostopic echo /arm_manipulator_command :

header: 
  seq: 633154
  stamp: 
    secs: 0
    nsecs:         0
  frame_id: "B_Link"
joint_names: [J1, J2, J3, J4, J5, J6]
points: 
  - 
    positions: [1.5707963267948966, 1.0471975511965976, 1.5707963267948966, 3.141592653589793, -1.0471975511965976, 1.5707963267948966]
    velocities: []
    accelerations: []
    effort: []
    time_from_start: 
      secs: 1
      nsecs:         0
  - 
    positions: [-1.5707963267948966, -1.0471975511965976, -1.5707963267948966, -3.141592653589793, 1.0471975511965976, -1.5707963267948966]
    velocities: []
    accelerations: []
    effort: []
    time_from_start: 
      secs: 2
      nsecs:         0
  - 
    positions: [3.141592653589793, 0.0, 0.0, 0.0, 1.0471975511965976, 0.0]
    velocities: []
    accelerations: []
    effort: []
    time_from_start: 
      secs: 3
      nsecs:         0

My code:

#include <ros/ros.h>
#include <trajectory_msgs/JointTrajectory.h>
#include "ros/time.h"
#include <iostream>
#include <math.h>

ros::Publisher arm_pub;
std::string joint_names[6] = { "J1", "J2", "J3", "J4", "J5", "J6" };

trajectory_msgs::JointTrajectoryPoint jointTrajectory_point(double time, int num_joints, double *joint_values){

    trajectory_msgs::JointTrajectoryPoint point;
    point.positions.resize(num_joints);
    for(int i=0; i<num_joints; i++) {
        point.positions[i] = joint_values[i];
    }
    point.time_from_start = ros::Duration(time);
    return point;
}

int main(int argc, char** argv) {
    ros::init(argc, argv, "state_publisher_node");
    ros::NodeHandle n;
    arm_pub = n.advertise<trajectory_msgs::JointTrajectory>("/arm_manipulator_controller/command",1);
    ros::Rate loop_rate(10);

    int num_joints = 6;

    trajectory_msgs::JointTrajectory traj;
    traj.header.frame_id = "B_Link";
    traj.header.stamp = ros::Time::now();

    traj.joint_names.resize(num_joints);
    for(int i=0;i<num_joints;i++){
        traj.joint_names[i] = joint_names[i];
    }

    traj.points.resize(3);

    double joint_values_1[6] = {M_PI/2, M_PI/3, M_PI/2, M_PI, -M_PI/3, M_PI/2}; 
    traj.points[0] = jointTrajectory_point(1.0, 6, joint_values_1);

    double joint_values_2[6] = {-M_PI/2, -M_PI/3, -M_PI/2, -M_PI, M_PI/3, -M_PI/2}; 
    traj.points[1] = jointTrajectory_point(2.0, 6, joint_values_2);

    double joint_values_3[6] = {M_PI, 0.0, 0.0, 0.0, M_PI/3, 0.0}; 
    traj.points[2] = jointTrajectory_point(3.0, 6, joint_values_3);

    while(ros::ok()) {
        arm_pub.publish(traj);
        ros::spinOnce();
        loop_rate.sleep();
    }
    return 0;
}

Your help would be much appreciated. I have already given a lot of time on this without any useful output.

Thanks in advance !!


Originally posted by Anubhav Singh on ROS Answers with karma: 67 on 2020-09-26

Post score: 0


Original comments

Comment by gvdhoorn on 2020-09-26:
If you are looking to execute trajectories, why are you using the command topic, instead of constructing a proper FollowJointTrajectory action goal, creating a client and then submitting the goal using the client to the JointTrajectory action server?

Comment by Anubhav Singh on 2020-09-26:
Thanks for replying. Yeah, that's what I am planning to try next. Above is what I was thinking of doing since a long time (in order to test robot trajectories in gazebo). So, I gave it a try.

$\endgroup$

1 Answer 1

0
$\begingroup$

Rosanswers logo

When I am running the node, my robotic arm is performing only first trajectory .

(note: I'll assume trajectory here should actually read trajectory point)

This is a guess, but I believe the following may explain what you observe:

  1. you are publishing messages to the command topic. This is a fire-and-forget interface to the JointTrajectoryController, meaning you get no feedback and are responsible yourself to make sure "the current trajectory" has finished executing before sending a new one
  2. if you don't check for completion of a trajectory, and send a new one while the controller is executing the last trajectory you sent, the controller will try to replace the current trajectory with the new one
  3. you are not sending "three trajectories", but only a single one -- with three trajectory points
  4. the while-loop in your main(..) causes the same trajectory to be published again and again at a frequency of 10 Hz

Taking all of this together, it's possible that what you see is completely according to how the JointTrajectoryController is specced to work:

  • the same trajectory is sent as input to the controller at 10 Hz, which causes the controller to attempt execution of that trajectory
  • while executing, it receives a new trajectory and will start to calculate how to join the two (ie: current and next trajectory)
  • the replacement trajectory is the same trajectory, and has the first time_from_start always set to 1.0
  • as 1.0 / 10 == 0.1 < 1.0, the first point of the trajectory will always be the destination point

In essence, you're giving the JointTrajectoryController a single destination (the first point in the trajectory) and always 1 second to reach it and tell it to go again to that point at a frequency of 10 Hz.

The other two trajectories are performed when I am pressing ctrl+c command

(again: I'll assume trajectory should actually read trajectory point)

This can be explained by the fact that once you stop your script, the JointTrajectoryController finally gets a chance to completely execute the trajectory (ie: without any replacement trajectories coming in), so it will (should) reach points 2 and 3 in 2 and 3 seconds respectively.

Finally: as I wrote in my initial comment: it's ok to use the command interface/topic, but you'll have to make sure the controller has executed the trajectory before sending a new one (or actually make use of the replacement capabilities of course).

If you're not interested in replacement, I would suggest you instead use the action interface which lets you wait on completion of a goal, notifying you when it's ok to send a new trajectory.


Originally posted by gvdhoorn with karma: 86574 on 2020-09-28

This answer was ACCEPTED on the original site

Post score: 2


Original comments

Comment by Anubhav Singh on 2020-09-29:
Thank you so much for your answer. I will try these points.

Comment by Contour on 2021-12-09:
I met the same problem. Using action interface actually solved it. Thanks a lot !

$\endgroup$

Your Answer

By clicking “Post Your Answer”, you agree to our terms of service and acknowledge you have read our privacy policy.