Robot Executes Only Part of TrajectoryGoal

[jbeck28]

I've included modified parts of the code given in the sample code (names a1_joint_trajectory_node.cpp, and corresponding RAPID program). The problem I'm having is that the robot only seems to execute the first point or two of 9 in the trajectory. I'm wondering if there is a maximum number of points that should be in each of the abb::egm::wrapper::trajectory::TrajectoryGoal objects? If not, any ideas as to the cause of this? The relevant code is here:

void goalCB() {
  ROS_INFO("asdf");
  goal_ = as_.acceptNewGoal();

  std::vector<trajectory_msgs::JointTrajectoryPoint> trajArray =
      goal_->trajectory.points;
  abb::egm::wrapper::trajectory::PointGoal* p_point;
  abb::egm::wrapper::trajectory::TrajectoryGoal trajectory_1;

  p_point = trajectory_1.add_points();
  // p_point->set_duration(2.5);
  double prevtime = 0;
  std::cout << trajArray.size() << "number of points in msg" << std::endl;
  for (int i = 0; i < trajArray.size(); i++) {
    // p_point->set_duration((trajArray[i].time_from_start.toSec()-prevtime)*10);
    p_point->set_duration(2.5);
    p_point->set_reach(true);

    std::cout << trajArray[i].time_from_start.toSec() - prevtime << " duration"
              << std::endl;

    prevtime = trajArray[i].time_from_start.toSec();
    for (int j = 0; j < trajArray[i].positions.size(); j++) {
      std::cout << "joint angle: " << trajArray[i].positions[j]
                << " Number of joints in pose " << i << " is "
                << trajArray[i].positions.size() << std::endl;
      p_point->mutable_robot()
          ->mutable_joints()
          ->mutable_position()
          ->add_values(trajArray[j].positions[j] * 180 / 3.1415926);
    }
    /*if(i%3 == 0){
            egm_interface.addTrajectory(trajectory_1);
    }*/
  }

  ROS_INFO("2: Add joint trajectories to the execution queue");
  egm_interface.addTrajectory(trajectory_1);
  ROS_INFO("3: Wait for the trajectory execution to finish...");
  abb::egm::wrapper::trajectory::ExecutionProgress execution_progress;
  wait = true;
  while (ros::ok() && wait) {
    ros::Duration(0.5).sleep();
    if (egm_interface.retrieveExecutionProgress(&execution_progress)) {
      wait = execution_progress.goal_active();
      std::cout << "goal is active" << std::endl;
    } else {
      std::cout << "goal is not active" << std::endl;
    }
  }
}

Thanks in advance!

[jbeck28]

This was correct (much to my surprise) by explicitly setting the joint angles rather than having the inner for-loop. For example:

`for(int i = 0;i<trajArray.size();i++){ p_point = trajectory_1.add_points(); if(i == 0) p_point->set_reach(true);

        //std::cout << trajArray[i].time_from_start.toSec()-prevtime<< " duration" << std::endl;
        //p_point->set_duration(2.5);
        p_point->set_duration((trajArray[i].time_from_start.toSec()-prevtime));

        p_point->mutable_robot()->mutable_joints()->mutable_position()->add_values(trajArray[i].positions[0]*180/pi);
        p_point->mutable_robot()->mutable_joints()->mutable_position()->add_values(trajArray[i].positions[1]*180/pi);
        p_point->mutable_robot()->mutable_joints()->mutable_position()->add_values(trajArray[i].positions[2]*180/pi);
        p_point->mutable_robot()->mutable_joints()->mutable_position()->add_values(trajArray[i].positions[3]*180/pi);
        p_point->mutable_robot()->mutable_joints()->mutable_position()->add_values(trajArray[i].positions[4]*180/pi);
        p_point->mutable_robot()->mutable_joints()->mutable_position()->add_values(trajArray[i].positions[5]*180/pi);

        prevtime = trajArray[i].time_from_start.toSec();
        /*for(int j = 0;j<trajArray[i].positions.size();j++){
            std::cout <<"joint angle: " <<trajArray[i].positions[j] << " Number of joints in pose "<< i<<" is " << trajArray[i].positions.size() <<  std::endl;
            p_point->mutable_robot()->mutable_joints()->mutable_position()->add_values(trajArray[j].positions[j]*180/3.14159265359);
        }*/

    }`

I'm curious why this worked.... But for the time being I'm satisfied with it.

[jontje]

Hello @jbeck28, I have been out of office and just got back today.

I tested your original code, and the problem was that you only created a single point in the EGM trajectory. I.e. p_point = trajectory_1.add_points(); was outside the outer loop.

In your second comment, I see that you have moved the creation of EGM points into the loop, which is why it works and that's how it should be.

Also, in your inner loop you were using j as index of trajArray, when mapping the values into the EGM trajectory point. You should be using i as index.

I hope this helps even though it's a bit late.

[jbeck28]

Thank you very much for your reply! You comment regarding add_points() makes sense. I wasn't entirely sure what I was doing... (hah).

I'm have another problem with my code, which is that the robot tends only to execute less than half the trajectories I send. In particular to tends not to do anything when there a large angular change in joint 6. Should I open a new issue, or is this related enough? Just in case, I've attached the code below. I'm attempting to write a driver that interface with moveit in the typical way, through an action server.

#include <ros/ros.h>
#include <abb_libegm/egm_trajectory_interface.h>
#include <actionlib/server/simple_action_server.h>
#include <control_msgs/FollowJointTrajectoryAction.h>
#include <trajectory_msgs/JointTrajectory.h>
#include <sensor_msgs/JointState.h>

#define pi 3.14159265359

void jointstatepub(abb::egm::EGMTrajectoryInterface* egm_interface){
    ros::NodeHandlePtr node = boost::make_shared<ros::NodeHandle>();
    ros::Publisher states_pub = node->advertise<sensor_msgs::JointState>("/joint_states", 1000);
    ros::Rate rate(100);
    abb::egm::wrapper::trajectory::ExecutionProgress progress;

    sensor_msgs::JointState msg;
    msg.header.seq=0;
    msg.name.push_back("joint_1");
    msg.name.push_back("joint_2");
    msg.name.push_back("joint_3");
    msg.name.push_back("joint_4");
    msg.name.push_back("joint_5");
    msg.name.push_back("joint_6");

      for(int k=0;k<6;k++)
      {
        msg.position.push_back(0);
        msg.velocity.push_back(0);
        msg.effort.push_back(0);
      }
      while(ros::ok())
      { 
          egm_interface->retrieveExecutionProgress(&progress);
         // std::cout<< "here"<<std::endl;
          msg.header.seq++;
          msg.header.stamp=ros::Time::now();

          for(int m=0;m<6;m++)
            {
             msg.position[m]=0.0174533*progress.inputs().feedback().robot().joints().position().values(m);
             msg.velocity[m]=progress.inputs().feedback().robot().joints().velocity().values(m);
            }
          states_pub.publish(msg);
          ros::Duration(.1).sleep();
          ros::spinOnce();
      }

}

class egmTrajectoryFollower{
protected: 
    ros::NodeHandle nh_;
    actionlib::SimpleActionServer<control_msgs::FollowJointTrajectoryAction> as_;
    std::string action_name_;
    boost::asio::io_service io_service;
    boost::thread_group thread_group;
    abb::egm::EGMTrajectoryInterface egm_interface;

    control_msgs::FollowJointTrajectoryGoalConstPtr goal_;
    control_msgs::FollowJointTrajectoryResult       result_;
    //control_msgs::FollowJointTrajectoryFeedback       feedback_;

public:
    bool wait = true;

    egmTrajectoryFollower(std::string name, int port):
        as_(nh_,name,false),
        action_name_(name),
        egm_interface(io_service,port)
    {

        /*if(!egm_interface.isInitialized())
          {
            ROS_ERROR("EGM interface failed to initialize (e.g. due to port already bound)");
            //return 0;
          }
        */
        thread_group.create_thread(boost::bind(&boost::asio::io_service::run, &io_service));
        as_.registerGoalCallback(boost::bind(&egmTrajectoryFollower::goalCB,this));
        as_.registerPreemptCallback(boost::bind(&egmTrajectoryFollower::preemptCB,this));
        //as_.registerFeedbackCallback(boost::bind(&egmTrajectoryFollower::feedbackCB,this));

        as_.start();

    ROS_INFO("1: Wait for an EGM communication session to start...");
      while(ros::ok() && wait)
      {
        if(egm_interface.isConnected())
        {
          if(egm_interface.getStatus().rapid_execution_state() == abb::egm::wrapper::Status_RAPIDExecutionState_RAPID_UNDEFINED)
          {
            ROS_WARN("RAPID execution state is UNDEFINED (might happen first time after controller start/restart). Try to restart the RAPID program.");
          }
          else
          {
            wait = egm_interface.getStatus().rapid_execution_state() != abb::egm::wrapper::Status_RAPIDExecutionState_RAPID_RUNNING;
          }
        }

        ros::Duration(0.5).sleep();
      }

      thread_group.create_thread(boost::bind(&jointstatepub,&egm_interface));

      std::cout << "Connected to controller" <<std::endl;

    }

    ~egmTrajectoryFollower(void){

    }

    void goalCB(){
        ROS_INFO("asdf");
        goal_ = as_.acceptNewGoal();

        std::vector<trajectory_msgs::JointTrajectoryPoint> trajArray = goal_->trajectory.points;
        abb::egm::wrapper::trajectory::PointGoal* p_point;
        abb::egm::wrapper::trajectory::TrajectoryGoal trajectory_1;
        //p_point->set_duration(2.5);
        double prevtime = 0;
        std::cout << trajArray.size() << "number of points in msg" << std::endl;
        for(int i = 0;i<trajArray.size();i++){
            p_point = trajectory_1.add_points();
            if(i == 0) p_point->set_reach(true);            

            //std::cout << trajArray[i].time_from_start.toSec()-prevtime<< " duration" << std::endl;
            //p_point->set_duration(2.5);
            p_point->set_duration((trajArray[i].time_from_start.toSec()-prevtime));

            p_point->mutable_robot()->mutable_joints()->mutable_position()->add_values(trajArray[i].positions[0]*180/pi);
            p_point->mutable_robot()->mutable_joints()->mutable_position()->add_values(trajArray[i].positions[1]*180/pi);
            p_point->mutable_robot()->mutable_joints()->mutable_position()->add_values(trajArray[i].positions[2]*180/pi);
            p_point->mutable_robot()->mutable_joints()->mutable_position()->add_values(trajArray[i].positions[3]*180/pi);
            p_point->mutable_robot()->mutable_joints()->mutable_position()->add_values(trajArray[i].positions[4]*180/pi);
            p_point->mutable_robot()->mutable_joints()->mutable_position()->add_values(trajArray[i].positions[5]*180/pi);

            prevtime = trajArray[i].time_from_start.toSec();
            /*for(int j = 0;j<trajArray[i].positions.size();j++){
                std::cout <<"joint angle: " <<trajArray[i].positions[j] << " Number of joints in pose "<< i<<" is " << trajArray[i].positions.size() <<  std::endl;
                p_point->mutable_robot()->mutable_joints()->mutable_position()->add_values(trajArray[j].positions[j]*180/3.14159265359);
            }*/

        }

        ROS_INFO("2: Add joint trajectories to the execution queue");
        egm_interface.addTrajectory(trajectory_1);

        ROS_INFO("3: Wait for the trajectory execution to finish...");
        abb::egm::wrapper::trajectory::ExecutionProgress execution_progress;
        wait = true;
        int j = 0;
        //ros::Duration(0.5).sleep();
        ros::Time end;
        ros::Time begin = ros::Time::now();
        while(ros::ok() && wait)
        {
            ros::Duration(0.025).sleep();
            if(egm_interface.retrieveExecutionProgress(&execution_progress))
            {
                wait = execution_progress.goal_active();
                //std::cout << execution_progress.pending_trajectories() << std::endl;
                if(j<20){
                    wait=true;
                    std::cout << "This goal is supposedly active. j = ";
                }
            }
            else if(j < 20)
            {
                wait = true;
                std::cout << "why is this goal not active? j = ";

            }
            std::cout << j << std::endl;
            j++;
        }
        end = ros::Time::now();

        if((end.toSec()-begin.toSec()) <= prevtime*1.6){
            as_.setSucceeded(result_);
            ROS_INFO("Execution Time was %f, expected was %f", end.toSec()-begin.toSec(), prevtime);
        }else{

            ROS_WARN("Execution Time was %f, expected was %f", end.toSec()-begin.toSec(), prevtime);
        }
    }

    void preemptCB(){
        ROS_INFO("Preempted");
        as_.setPreempted(result_);
    }

};

int main(int argc, char** argv){
    ros::init(argc,argv,"egm_server");
    egmTrajectoryFollower egmTrajectoryFollower("/abb_egm/joint_trajectory_action",6511);
    ros::spin();

    return 0;
}

Will EGM not execute a motion if the time constraint requires too fast of a motion?

Again, many thanks for your help, it is much appreciated!!!

[jontje]

Thank you very much for your reply! You comment regarding add_points() makes sense. I wasn't entirely sure what I was doing... (hah).

You are welcome 😃

I'm have another problem with my code, which is that the robot tends only to execute less than half the trajectories I send. In particular to tends not to do anything when there a large angular change in joint 6.

Hm, I just tested your code with a dummy trajectory that moves joint 6 several times (with a max deviation of about 300 degrees). See RobotStudio recording.zip.

It went fine for me, but I am using the StateMachine Add-In for setting up the RAPID program and system configurations. Also, I tested against a RobotWare 6.08.01 virtual controller.

Should I open a new issue, or is this related enough?

This question is related enough.

I'm attempting to write a driver that interface with moveit in the typical way, through an action server.

Aha, ok, you might want to take a look at this repo. It is for a ros_control based driver and it might already do the thing you want. I have not had time to test it myself yet, but I helped out with the correct usage of abb_libegm APIs there.

Will EGM not execute a motion if the time constraint requires too fast of a motion?

It depends the robot type you are using, but the RAPID program will trigger an error if the references are bad. It also depends on what parameters was used for the EGM RAPID instructions. Look at the EGMActJoint and EGMRunJoint parameters in particular (e.g. in the RAPID manual).

FYI: I will start my vacation today, and I will not be available for further questions until August.

[jbeck28]

Hmm, that's promising, and probably indicates I've done something wrong while setting up the RAPID side of this code. I essentially copied the TRob1Main.mod from your example code which was provided in a different issue thread. I guess I'm not entirely sure how to make use of the state machine add-in. I do have it installed, however. What RAPID code did you use when testing my code if you don't mind my asking?

Enjoy your vacation! I'll look through the repo you referenced here if I can't get my implementation to work.

[jontje]

I guess I'm not entirely sure how to make use of the state machine add-in. I do have it installed, however.

You can look at the Add-In's User Manual. There are instructions there for how to install a new system that includes the Add-In.

What RAPID code did you use when testing my code if you don't mind my asking?

I used the RAPID code that is automatically loaded by the Add-In during system installation. I only changed the DEFAULT_MAX_SPEED_DEVIATION constant in the TRobEGM module.

[jbeck28]

hmm, I now see those modules loaded in my controller (though I'm not sure how to locate them from robotstudio). I apologize for how simplistic a question this is, but I don't see how to actually run those modules, as they have no main. do I still need to have some sort of RAPID module that has a main, and uses those processes defined in the module TRobEGM_single?

[jontje]

The Add-In should have loaded these modules:

The TRobMain module contains the RAPID main procedure, and after starting the program, then it will wait for a trigger from an IO-signal before calling the procedures defined in the other modules.

See the RobotStudio Simulation section in the manual for the related IO-signals. If you test with a real robot, then you can for example trigger the IO-signals manually from the FlexPedant.

[jbeck28]

Since last posting I have gotten the trajectory interface to work reasonably okay. However, after calling p_point->set_reach(true) for each point in the trajectory, the robot seems to briefly stop at each point. I'm also curious if the durations, velocities and accelerations have any impact on execution when using trajectory_interface?

jontje, your last post over a month ago was a massive help, I did not have those files upon installing the state machine, so something had gone wrong there.

[jontje]

Durations, velocities and accelerations affect the interpolation calculated in egm_interpolator.cpp. For example, see L120-L279 for polynomial (used for joints and linear values) and L349-L382 for Slerp (used for quaternions). The actual interpolation method used depends on the configuration of the EGMTrajectoryInterface instance. See the TrajectoryConfiguration class for available configurations (e.g. can be specified when instantiating the interface).

During the EGM communication, then only position and (optionally) velocities are sent to the robot and these values are calculated based on the interpolation. Velocities are only sent to the robot if it has been specified in the configuration above.

The set_reach(...) method is intended to be used when you really want to reach a point. The condition for if a point has been reached is calculated here L348-L381, but how quickly the robot manage to get to the point depends a lot on the arguments specified in the RAPID instructions (e.g. position correction gain and the max allowed speed deviation).

The EGMTrajectoryInterface instance will not continue towards the next trajectory point, if the current point hasn't been reached. So, if you don't want that behaviour, then you can set the reach flag to false instead. This should make the motion execution smoother.

I hope this helps!

[gavanderhoorn]

Assuming this is now resolved, I'm closing this.

@jbeck28: if you're still having trouble getting things to work, please clarify and re-open.