Cannot cennect to Followjointtrajectoryaction in CPP for the ur_modern_driver

[Chunting]

Hi, Anyone has developed a action client in C++? I transfer the test_move.py to a cpp file, but it does not work, it seems that it can not connect to the action server. It stops at below.

while(!traj_client_->waitForServer(ros::Duration(5.0))){
      ROS_INFO("Waiting for the joint_trajectory_action server");
    }

#include <ros/ros.h>
#include <actionlib/client/simple_action_client.h>
#include <actionlib/client/terminal_state.h>
#include <std_msgs/String.h>
#include <control_msgs/FollowJointTrajectoryAction.h>

typedef actionlib::SimpleActionClient< control_msgs::FollowJointTrajectoryAction> TrajClient;

class RobotArm
{
private:
  // Action client for the joint trajectory action 
  // used to trigger the arm movement action
  TrajClient* traj_client_;

public:
  //! Initialize the action client and wait for action server to come up
  RobotArm() 
  {
    // tell the action client that we want to spin a thread by default
    traj_client_ = new TrajClient("follow_joint_trajectory", true);

    // wait for action server to come up
    while(!traj_client_->waitForServer(ros::Duration(5.0))){
      ROS_INFO("Waiting for the joint_trajectory_action server");
    }
  }

  //! Clean up the action client
  ~RobotArm()
  {
    delete traj_client_;
  }

  //! Sends the command to start a given trajectory
  void startTrajectory(control_msgs::FollowJointTrajectoryGoal goal)
  // void startTrajectory(pr2_controllers_msgs::JointTrajectoryGoal goal)
  {
    // When to start the trajectory: 1s from now
    goal.trajectory.header.stamp = ros::Time::now() + ros::Duration(1.0);
    traj_client_->sendGoal(goal);
  }

  //! Generates a simple trajectory with two waypoints, used as an example
  /*! Note that this trajectory contains two waypoints, joined together
      as a single trajectory. Alternatively, each of these waypoints could
      be in its own trajectory - a trajectory can have one or more waypoints
      depending on the desired application.
  */
  control_msgs::FollowJointTrajectoryGoal armExtensionTrajectory()
  {
    //our goal variable
    control_msgs::FollowJointTrajectoryGoal goal;

    // First, the joint names, which apply to all waypoints
    goal.trajectory.joint_names.push_back("shoulder_pan_joint");
    goal.trajectory.joint_names.push_back("shoulder_lift_joint");
    goal.trajectory.joint_names.push_back("elbow_joint");
    goal.trajectory.joint_names.push_back("wrist_1_joint");
    goal.trajectory.joint_names.push_back("wrist_2_joint");
    goal.trajectory.joint_names.push_back("wrist_3_joint");

    // We will have two waypoints in this goal trajectory
    goal.trajectory.points.resize(2);

    // First trajectory point
    // Positions
    int ind = 0;
    goal.trajectory.points[ind].positions.resize(6);

    goal.trajectory.points[ind].positions[0] = 2.2;
    goal.trajectory.points[ind].positions[1] = 0.0;
    goal.trajectory.points[ind].positions[2] = -1.57;
    goal.trajectory.points[ind].positions[3] = 0.0;
    goal.trajectory.points[ind].positions[4] = 0.0;
    goal.trajectory.points[ind].positions[5] = 1.0;
    // Velocities
    goal.trajectory.points[ind].velocities.resize(6);
    for (size_t j = 0; j < 6; ++j)
    {
      goal.trajectory.points[ind].velocities[j] = 0.0;
    }
    // To be reached 1 second after starting along the trajectory
    goal.trajectory.points[ind].time_from_start = ros::Duration(1.0);

    // Second trajectory point
    // Positions
    ind += 1;
    goal.trajectory.points[ind].positions.resize(6);
    goal.trajectory.points[ind].positions[0] = -0.3;
    goal.trajectory.points[ind].positions[1] = 0.2;
    goal.trajectory.points[ind].positions[2] = -0.1;
    goal.trajectory.points[ind].positions[3] = -1.2;
    goal.trajectory.points[ind].positions[4] = 1.5;
    goal.trajectory.points[ind].positions[5] = -0.3;
    // Velocities
    goal.trajectory.points[ind].velocities.resize(6);
    for (size_t j = 0; j < 6; ++j)
    {
      goal.trajectory.points[ind].velocities[j] = 0.0;
    }
    // To be reached 2 seconds after starting along the trajectory
    goal.trajectory.points[ind].time_from_start = ros::Duration(2.0);

    //we are done; return the goal
    return goal;
  }

  //! Returns the current state of the action
  actionlib::SimpleClientGoalState getState()
  {
    return traj_client_->getState();
  }

};

int main(int argc, char** argv)
{
  // Init the ROS node
  ros::init(argc, argv, "ur_driver");

  RobotArm arm;
  // Start the trajectory
  arm.startTrajectory(arm.armExtensionTrajectory());
  // Wait for trajectory completion
  while(!arm.getState().isDone() && ros::ok())
  {
    usleep(50000);
  }
}

I am just a beginner for ROS and UR. And would you please tell how to intergrate other package such as ur_kinematics into test_move.py? where can I find an example for UR5? Thanks for your help.

Sincerely, Chunting

[Chunting]

Thank you all the same, I have figured it out~~~

[ThomasTimm]

Great, I'll close this issue then. Next time, you should probably direct your questions to either

• ROS mailing list
• ROS Industrial mailing list
• MoveIt! mailing lsit
• ROS answers

when you are in doubt on how to accomplish something, as these questions are not an issue with this specific package

[Chunting]

OK. Thanks for your information. You are so kind.

Chunting

2016-05-30 16:34 GMT+08:00 Thomas Timm Andersen notifications@github.com:

Great, I'll close this issue then. Next time, you should probably direct your questions to either

• ROS mailing list
• ROS Industrial mailing list
• MoveIt! mailing lsit
• ROS answers

when you are in doubt on how to accomplish something, as these questions are not an issue with this specific package

— You are receiving this because you authored the thread. Reply to this email directly, view it on GitHub https://github.com/ThomasTimm/ur_modern_driver/issues/52#issuecomment-222441815, or mute the thread https://github.com/notifications/unsubscribe/ADKLGa2GlSEsHDTo_TJUy5KNaW6bDkMwks5qGqEtgaJpZM4IpA6T .

焦春亭

[Chunting]

Hi Thomas, Would you please send me some materials about ur_modern_driver, such as research papers or your thesis? I am a Ph.D. student from Tsinghua University, and my research field is compliance force control for the target capture. I want to develop my experiment based on your driver, after studing it for several days, I am still a little confused about some questions, so I need your help. Now I have read your docs OPTIMAIZING THE UNIVERSAL ROBOTS ROS DRIVER, but would you please give me some more details about your controller? Or would your please give me some other experiment examples to help it more understandable? I learned that you have just submitted your PHD thesis, can I take a look at it?

Yor are so kind to make your driver open, and it is a great job. Thank you very much.

Sincerely, Chunting

2016-05-30 16:34 GMT+08:00 Thomas Timm Andersen notifications@github.com:

Great, I'll close this issue then. Next time, you should probably direct your questions to either

• ROS mailing list
• ROS Industrial mailing list
• MoveIt! mailing lsit
• ROS answers

when you are in doubt on how to accomplish something, as these questions are not an issue with this specific package

— You are receiving this because you authored the thread. Reply to this email directly, view it on GitHub https://github.com/ThomasTimm/ur_modern_driver/issues/52#issuecomment-222441815, or mute the thread https://github.com/notifications/unsubscribe/ADKLGa2GlSEsHDTo_TJUy5KNaW6bDkMwks5qGqEtgaJpZM4IpA6T .

焦春亭

[ThomasTimm]

Chunting, I haven't made any other documentation than the paper you've already read. if using the driver in your research, please cite that paper. My phd thesis is available from http://orbit.dtu.dk/en/publications/sensor-based-realtime-control-of-robots(34ded5f8-4cae-4bb0-82fa-35d432dfcf07).html but it doesn't really give any further information than what is in the paper Optimizing the universal robots ROs driver It will probably be easier for both of us if I answer your questions rather than me trying to guess what you want to know.

[Chunting]

Hi Thomas, Would you please help me explain the control strategy of your controller in ur_modern_driver? Is it open or closed loop? Do you have the block scheme of the system? Since I want to integrate your driver into force control, I want to learn the mathematic relation in position and velocity (acceleration) level, then I think I can make the controller better.

Thanks,

Regards, Chunting

2016-06-13 15:29 GMT+08:00 Thomas Timm Andersen notifications@github.com:

Chunting, I haven't made any other documentation than the paper you've already read. if using the driver in your research, please cite that paper. My phd thesis is available from http://orbit.dtu.dk/en/publications/sensor-based-realtime-control-of-robots(34ded5f8-4cae-4bb0-82fa-35d432dfcf07).html but it doesn't really give any further information than what is in the paper Optimizing the universal robots ROs driver It will probably be easier for both of us if I answer your questions rather than me trying to guess what you want to know.

— You are receiving this because you authored the thread. Reply to this email directly, view it on GitHub https://github.com/ThomasTimm/ur_modern_driver/issues/52#issuecomment-225507234, or mute the thread https://github.com/notifications/unsubscribe/ADKLGQw7BDgAVybO38X_SEsXvJkpMzIfks5qLQbQgaJpZM4IpA6T .

焦春亭

[ThomasTimm]

I haven't implemented any controllers, the driver is only an interface to the robot. Thus I guess you can say that the driver is open-loop, but it is inteded to be sued with a ros_control controller which closes the loop. Also, there is probably one or many controllers running on the robot itself, trying to keep the robot moving at the commanded velocity. How they work is not known.