0
$\begingroup$

Rosanswers logo

Dear community,

  1. I'm absolutely new to ROS, so please excuse my technically perhaps not always quite correct expressions.
  2. I'm running ROS Kinetic on Ubuntu 16.04 LTS and cloned the "universal robot" package from the following GitHub repository: (https://github.com/ros-industrial/universal_robot.git)
  3. After "building" my Catkin workspace by typing catkin_make everything worked fine so far and I could successfully launch a simulation of an ur3 robot in Gazebo by typing roslaunch ur_gazebo ur3.launch into a new terminal.
  4. What i wanted to do now, was to control the total of six joints of the ur3 without using MoveIt. So i did some research and found the following Link: Moving the arm using the Joint Trajectory Action So basically I wanted to try to control the ur3 in the Gazebo simulation via an action client. Since the description in the link above did not exactly fit to my robot, I tried to adapt the simple_trajectory.cpp file written there to the ur3 robot.

My "adapted" code file (without the included headers) now looks like shown below.

(The libraries I included by writing #include are the following: <ros/ros.h>; <control_msgs/FollowJointTrajectoryAction.h> and <actionlib/client/simple_action_client.h)

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

class Ur3Arm
{
    private:
        // Action client for the follow 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.
        Ur3Arm()
        {
            // Tell the action client that we want to spin a thread by default:
            traj_client_ = new TrajClient("arm_controller/follow_joint_trajectory_action", true);
            
            // Wait for the action server to come up
            while(!traj_client_->waitForServer(ros::Duration(5.0))) 
            {
                ROS_INFO("Wait for the follow_joint_trajectory_action server");
            }
        }
        
        // Clean up the action client
        // -> Destructor of class Ur3Arm
                
        ~Ur3Arm()
        {
            delete traj_client_;
        }
        
        // Send the command to start a given trajectory
        
        void startTrajectory(control_msgs::FollowJointTrajectoryGoal 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()
        {
            // The goal variable:
            control_msgs::FollowJointTrajectoryGoal goal;
            
            // First, the joint names, which apply to all waypoints
            goal.trajectory.joint_names.resize(1);
            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");
            
            // In this simple case there asre only two waypoints in this goal trajectory
            goal.trajectory.points.resize(2);
            
            // First trajectory waypoint
                        
                        int ind = 0; // Represents the number of the actual waypoint
                        
                        // Positions:
                        
                        goal.trajectory.points[ind].positions.resize(6);
                        goal.trajectory.points[ind].positions[0] = 0.0;
                        goal.trajectory.points[ind].positions[1] = 0.0;
                        goal.trajectory.points[ind].positions[2] = 0.0;
                        goal.trajectory.points[ind].positions[3] = 0.0;
                        goal.trajectory.points[ind].positions[4] = 0.0;
                        goal.trajectory.points[ind].positions[5] = 0.0;
            
                        // Velocities
                        goal.trajectory.points[ind].velocities.resize(6);
                        for(size_t i = 0; i < 6; ++i)
                        {
                            goal.trajectory.points[ind].velocities[i] = 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 waypoint
            
                        ind += 1;
                        // Positions:
                        goal.trajectory.points[ind].positions.resize(6);
                        goal.trajectory.points[ind].positions[0] = 0.0;
                        goal.trajectory.points[ind].positions[1] = -1.5;
                        goal.trajectory.points[ind].positions[2] = 1.5;
                        goal.trajectory.points[ind].positions[3] = -3.0;
                        goal.trajectory.points[ind].positions[4] = 0.0;
                        goal.trajectory.points[ind].positions[5] = 0.0;
            
                        // Velocities
                        goal.trajectory.points[ind].velocities.resize(6);
                        for(size_t i = 0; i < 6; ++i)
                        {
                            goal.trajectory.points[ind].velocities[i] = 0.0;
                        }
            
                        // To be reached 2 second after starting along the trajectory
                        goal.trajectory.points[ind].time_from_start = ros::Duration(2.0);
                        
                        // Function 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, "robot_driver");
    
    Ur3Arm arm;
    
    // Start the trajectory
    arm.startTrajectory(arm.armExtensionTrajectory());
    
    // Wait for trajectory completion
    while(!arm.getState().isDone() && ros::ok())
    {
        usleep(50000); // Argument of usleep is given in miliseconds -> Wait fo 50s
    }
    
    return 0;
}

The code can be compiled succesfully and I can also run the action client by typing rosrun simple_trajectory simple_trajectory_action_client (My package ist named simple_trajectory)

The problem I have is, that the code obviously stucks at the following point

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

since this message appears on my terminal every five seconds. Of course each action client needs an action server, but like I understood the whole thing, the action server starts automatically by launching the ur3 robot in Gazebo, right? I think so, because after launching the ur3 robot in Gazebo I got the following output by typing rostopic list |grep arm_controller in a terminal:

/arm_controller/command
/arm_controller/follow_joint_trajectory/cancel
/arm_controller/follow_joint_trajectory/feedback
/arm_controller/follow_joint_trajectory/goal
/arm_controller/follow_joint_trajectory/result
/arm_controller/follow_joint_trajectory/status
/arm_controller/state

Can anyone tell me if this way to move the ur3 robots arm can work at all and if so, where the problem in my implementation is? I'd be very grateful for any help!

Edit: Hi jschornak,

First of all thank you very much for your quick help! You made me understand the whole thing a little bit better now. After fixing the wrong implementation of the line where I create the new action client to

 traj_client_ = new TrajClient("/arm_controller/follow_joint_trajectory", true);

I can now succesfully run the action client without stucking at the point, where the client waits for the action server. So your advice could fix my original problem! So far, so good!...

Unfortunately, I now have the problem that after starting the action client by typing

rosrun simple_trajectory simple_trajectory (package an file are both named like this)

the terminal gets "busy" for about one second while the action client "executes", but the robots arm doesn't seem to get any moving commands in Gazebo. After this short period of time, my simple_trajectory.cpp file has been completely executed, the terminal where i executed the file gets "free" again and nothing happened. I really hope, I made my problem clear at this point.

Can you or anyone else tell me where the problem is?

Thank you in advance!

Originally posted by ROSDeveloper1996 on ROS Answers with karma: 23 on 2019-11-04

Post score: 2

Original comments

Comment by gvdhoorn on 2019-11-05:
Please do not post answers, unless you are answering your own questions.

For interacting with other posters, please use comments, or edit your original question to add a section to it. Use the edit button/link for that.

Improve this question
$\endgroup$

1 Answer 1

Reset to default
0
$\begingroup$

Rosanswers logo

Make sure that the action name you use when you create the client matches the name of the action provided by the server. Since the action topics that were listed when you used rostopic list are all under /arm_controller/follow_joint_trajectory, there are two things wrong with this line:

traj_client_ = new TrajClient("arm_controller/follow_joint_trajectory_action", true);
  1. Instead of follow_joint_trajectory_action, you should use follow_joint_trajectory to match the name advertised by the server.
  2. If you begin a ROS topic name without a slash (/) then the name will be evaluated as a relative name under the node's namespace. Because the node with the client is named robot_driver, as currently written the action client is looking for an action named /robot_driver/arm_controller/follow_joint_trajectory_action, which doesn't exist. There's a wiki page with an overview of ROS naming rules here.

After fixing these issues the line where you create the action client should look like this:

    traj_client_ = new TrajClient("/arm_controller/follow_joint_trajectory", true);

Originally posted by jschornak with karma: 340 on 2019-11-04

This answer was ACCEPTED on the original site

Post score: 2

Improve this answer
$\endgroup$

Your Answer

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