EC66 and EC612 ROS driver tests

[progettista6]

Hello,

I performed some tests on an EC66 and an EC612 robots and I noticed very different behaviors not only using transparent transmission or the SDK functionalities, but also between the two robots.

Let me explain better what I noticed.

EC612 robot:

• the velocity and acceleration values in the teach pendant and in the file joint_limits.yaml correspond, as you can see from the images "ec612_jointlimits.png", "teach_ec612_acc.png" and "teach_ec612_vel.png".
• Launching the node "elite_axis_ROS1.cpp", which uses the "joint_move" service to command movements to the robot (so the SDK functionalities) I noticed that some movements are slower, while setting the same velocity and acceleration factors;
• Launching the node "elite_statico_ROS1.cpp", which uses the transparent transmission functionalities, I noticed slow and non-fluent movements. Moreover, they are slower than in the previous configuration.
• If I set 100% as speed scaling factor and 50% as acceleration factor, I have the errors reported in the image "ec612_errors.png", while setting lower factors the robot skips some positions. This is dangerous, because the robot or the gripper could hit obstacles.

EC66 robot:

• I noticed that the speed values setted in the file joint_limits.yaml doesn't match the ones reported in the teach pendant, in fact these latter are slower (as you can see from images "teach_ec66_vel.jpg" and "ec66_jointlimitsacc.jpg"/"ec66_jointlimitsnoacc.jpg");
• In the node I used to perform the test, the first two movements use the SDK functionalities, while the last two use the transparent transmission functionalities;
• I noticed two different behaviours setting or not the acceleration limits in the joint_limits.yaml file. Setting the acceleration limits as indicated in the teach pendant (image "teach_ec66_acc.jpg") and 100% velocity, in both configurations a speed error is reported (image "ec66_speederror.jpg"). Without accelerations limits, and 100% velocity, using SDK functionalities, the robot moves well and I don't notice any particular difference in movements/speed/accelerations, while using transparent transmission functionalities, the robot moves well but slower than in the previous configuration.

I attach the three .cpp files (in .txt format that is supoorted here in GitHub).

ec66_jointlimitsacc

ec66_jointlimitsnoacc

ec66_speederror

ec612_jointlimits

teach_ec66_acc

teach_ec66_vel

teach_ec612_acc

teach_ec612_vel

ec66_prova.txt elite_axis_ROS1.txt elite_statico_ROS1.txt

Waiting for your feedback, Thank you

[padhupradheep]

Hey, Is there any solution on this already?

[miaokeqin]

I tested your code of elite_axis_ROS1 on the ec66 robot (the other two codes seem to be incomplete), set the speed of the teach pendant to 100%, use 100% speed and 50% acceleration in the code, and there is no alarm mistake. I don't know where the transparent transmission function you mentioned is implemented. In this file, the transparent transmission is not used to operate the robot.

[progettista6]

That's my fault, sorry.

I attach the missing files below, with which the other two nodes can also be tested.

elite.h

#ifndef ELITE_H
#define ELITE_H

#include "trajectory_msgs/JointTrajectory.h"
#include "trajectory_msgs/JointTrajectoryPoint.h"
#include "control_msgs/JointTrajectoryControllerState.h"

#include <ros/ros.h>

#include <moveit/move_group_interface/move_group_interface.h>
#include <moveit/planning_scene_interface/planning_scene_interface.h>
#include <tf/transform_broadcaster.h>

class Elite{
    private: 
        const int max_attempts = 3;
        ros::NodeHandle& nh;

    public:

        Elite(moveit::planning_interface::MoveGroupInterface& move_group, ros::NodeHandle& nh_);
        bool moveToTargetPose( const std::string named_target);

        bool setTargetJointValues( const std::vector<double> &des_joint_values);
        bool moveJoints(const std::vector<double> &des_joint_values);
        void buildUpMap();
        std::map<std::string, std::vector<double> >vect_cord_pose;

        moveit::planning_interface::MoveGroupInterface& move_group;
        moveit::planning_interface::MoveGroupInterface::Plan target_pose_plan;
};

#endif

elite.cpp

#include "elite.h"

Elite::Elite(moveit::planning_interface::MoveGroupInterface& move_group, ros::NodeHandle& nh_): 
    nh(nh_),
    move_group( move_group )
{
    buildUpMap();
}

bool Elite::moveToTargetPose(const std::string named_target)
{
    for(int n=0;n<max_attempts;n++)
    {
        ROS_INFO("[elite] attempt: %d", n+1);
        move_group.setNamedTarget(named_target);
        move_group.setStartState(*move_group.getCurrentState());
        if(move_group.plan(target_pose_plan)==moveit::planning_interface::MoveItErrorCode::SUCCESS)
        {
            move_group.execute(target_pose_plan);
            return true;
        }
        ROS_FATAL("[elite]: unable to find a path for the given target pose");
        return false;
    }
}

bool Elite::setTargetJointValues( const std::vector<double> &des_joint_values )
{
    moveit::core::RobotStatePtr current_state = move_group.getCurrentState();
    return move_group.setJointValueTarget( des_joint_values ) == moveit::planning_interface::MoveItErrorCode::SUCCESS;
}

bool Elite::moveJoints(const std::vector<double> &d_j_values)
{

    if( setTargetJointValues( d_j_values ) ) {
        ROS_INFO( "JOINT GOAL: Successfuly generated JointPlan! Move to desired JointValues..." );
        move_group.move();
        return true;
    }
    else {
        ROS_FATAL( "JOINT GOAL: Not possible to find an executable trajectory path" );
        return false;
    }
}

void Elite::buildUpMap()
{
    //-----EC612 positions-----
    vect_cord_pose.insert( std::pair<std::string, std::vector<double> >("pre_scatola1", {1.936460258,-0.864950271,1.63280788,-2.341394098,1.57259402,-1.95023091}) );
    vect_cord_pose.insert( std::pair<std::string, std::vector<double> >("scatola1", {1.936512618,-0.375367962,1.51618497,-2.714370959,1.57257656,-1.950196}) );
    vect_cord_pose.insert( std::pair<std::string, std::vector<double> >("pre_scatola2", {2.024337586,-0.46321038,0.828280903,-1.938781546,1.57233222,-1.862353578}) );
    vect_cord_pose.insert( std::pair<std::string, std::vector<double> >("scatola2", {2.011683949,-0.042114795,0.6318092,-2.161049227,1.57348413,-1.877939369}) );
    vect_cord_pose.insert( std::pair<std::string, std::vector<double> >("pre_scatola3", {2.352808551,-0.863641274,1.63027715,-2.340678513,1.5713199,-1.53390007}) );
    vect_cord_pose.insert( std::pair<std::string, std::vector<double> >("scatola3", {2.352826005,-0.374582564,1.51360189,-2.713079416,1.5713199,-1.53388261}) );
    vect_cord_pose.insert( std::pair<std::string, std::vector<double> >("pre_scatola4", {2.346979152,-0.46146505,0.824737885,-1.93732,1.57133738,-1.53971201}) );
    vect_cord_pose.insert( std::pair<std::string, std::vector<double> >("scatola4", {2.346996605,-0.048310714,0.65118234,-2.176931723,1.57133738,-1.53971201}) );  
    vect_cord_pose.insert( std::pair<std::string, std::vector<double> >("pre_place", {3.146252683,-0.51679199,0.660275509,-1.65251264,1.64230247,-0.74249797}) );
    vect_cord_pose.insert( std::pair<std::string, std::vector<double> >("place_box", {3.146252683,-0.376694412,0.767281646,-1.900384303,1.64230247,-0.742480517}) );

    vect_cord_pose.insert( std::pair<std::string, std::vector<double> >("scatola1a", {-1.23838092,-0.375367962,1.51618497,-2.714370959,1.57257656,-1.950196}) );
    vect_cord_pose.insert( std::pair<std::string, std::vector<double> >("pre_scatola1a", {-1.23832856,-0.864950271,1.63280788,-2.341394098,1.57259402,-1.95023091}) );
    vect_cord_pose.insert( std::pair<std::string, std::vector<double> >("pre_scatola2a", {-1.15167296,-0.46321038,0.828280903,-1.938781546,1.57233222,-1.862353578}) );
    vect_cord_pose.insert( std::pair<std::string, std::vector<double> >("scatola3a", {-0.816936263,-0.374582564,1.51360189,-2.713079416,1.5713199,-1.53388261}) );
    vect_cord_pose.insert( std::pair<std::string, std::vector<double> >("pre_scatola3a", {-0.81691881,-0.863641274,1.63027715,-2.340678513,1.5713199,-1.53390007}) );
    vect_cord_pose.insert( std::pair<std::string, std::vector<double> >("scatola4a", {-0.811106863,-0.048310714,0.65118234,-2.176931723,1.57133738,-1.53971201}) );
    vect_cord_pose.insert( std::pair<std::string, std::vector<double> >("pre_scatola4a", {-0.81108941,-0.46146505,0.824737885,-1.93732,1.57133738,-1.53971201}) );
    vect_cord_pose.insert( std::pair<std::string, std::vector<double> >("scatola2a", {-1.13901932,-0.042114795,0.6318092,-2.161049227,1.57348413,-1.877939369}) );
    vect_cord_pose.insert( std::pair<std::string, std::vector<double> >("pre_pre_place",{-0.785398,-1.5708,1.5708,-1.5708,1.5708,-1.5708})); 

    //-----EC66 positions-----
    vect_cord_pose.insert( std::pair<std::string, std::vector<double> >("ec66_home",{2.106158622,-1.5708,1.5708,-1.5708,1.5708,-1.5708}));
    vect_cord_pose.insert( std::pair<std::string, std::vector<double> >("ec66_pos1",{0.010157816,-2.106158622,1.841601614,-1.35207166,1.5831882,-3.139009566}));
}

Thank you and best regards

[miaokeqin]

OK, I'll test it and get back to you

[miaokeqin]

For the two interface problems, the waypoints with time stamps planned by ros are polynomially fitted to obtain the joint position, velocity and acceleration. Due to the different interfaces, the speed will be slightly slower. If the points are reduced, the speed can be increased but the mechanical arm The waypoints become imprecise and are not recommended.

[padhupradheep]

@miaokeqin

We also use the EC 66 arm at Neobotix. We also have the same issue. First of all, I would like to describe about the cases where the planning and execution does not have any issues and later the non-working issue.

The first case is the situation where we try to send point to point commands. We use RViz and set a valid goal position for the robot, then the goal is reached as expected. But it is totally understandable that we use the position controller, and even the MoveIt sends the position commands. But you have to note that, there is an acceleration limit and the velocity limit which the MoveIt considers while generating the list of poses, or maybe simply a trajectory. When these positions are fed to arm through the SDK, we suspect from our visual inference that, they go beyond the designated acceleration limit given in the MoveIt yaml file. Then a cubic interpolator is used to handle these poses and then fed into the SDK.

Second case is the situation where we try to send in joint commands for following a sine-wave (a set of Cartesian waypoints). We use the Cartesian planner which eventually generates joint commands for reaching the desired Cartesian goal location. In this case, the robot arm reaches the alarm as soon as the robot starts moving, especially joint 3.

The overall point is that, both the velocity limits and the acceleration limits are not considered at all while giving the position commands. I guess, this is somehow has to deal with the interpolator. Moveit will not be anyways generating a trajectory that the robot arm cannot follow. In our scenario we do set the limits, irrespective it fails. I feel there might be a way to handle it.

[progettista6]

For the two interface problems, the waypoints with time stamps planned by ros are polynomially fitted to obtain the joint position, velocity and acceleration. Due to the different interfaces, the speed will be slightly slower. If the points are reduced, the speed can be increased but the mechanical arm The waypoints become imprecise and are not recommended.

We would also like to use this second trajectory planning mode. Are you thinking of developing something about it to make it usable?

[miaokeqin]

After testing, we also found this problem, but this problem requires time to modify the content in elite_controller, we will test and modify it as soon as possible

[miaokeqin]

@progettista6 Hello, I also want to ask if you want to use moveit to control the robot to complete the task, and then find that the speed is too slow? We do not recommend directly modifying the joint_limit file. By modifying the controller parameters, the speed can be increased (see joint_trajectory_action.py ), but sometimes an error occurs.

This coefficient should not be less than 0.55

The speed of moveit can be nearly the same as the speed planned in move_group/result and the actual execution speed of the robot.

When I tested with the code you provided, there was no overspeed alarm when the joint_limit was not modified. For el.setMaxAccelerationScalingFactor(); And el.setMaxVelocityScalingFactor(); The coefficient of the interface has the function of speed regulation, you can set 0.1 and 1 for comparison

[progettista6]

Sorry, but in the file joint_trajectory_action.py in the repository I don't find the coefficient you mentioned, as you can see from the image. Should this only be added there, or also in other files?

Thank you

[miaokeqin]

Yes, this coefficient is added by yourself, such as *0.7

[progettista6]

After testing, we also found this problem, but this problem requires time to modify the content in elite_controller, we will test and modify it as soon as possible

Hello, is there any news about this?

Yes, this coefficient is added by yourself, such as *0.7

So, I have to add it only in the file you mentioned or also in other files? Can you give me a list of any changes to be made? Thank you

[miaokeqin]

Yes, it is to add coefficients to this file to shorten the time of the timestamp to speed up the movement of the robot

[miaokeqin]

At present, there is no plan to rewrite a new controller, which is time-consuming and laborious

[padhupradheep]

Yes, it is to add coefficients to this file to shorten the time of the timestamp to speed up the movement of the robot

And to decrease the speed? increase it right?

@miaokeqin could you maybe give us some reference on how you came up with this specific controller. Like maybe a small introduction to the theory on what type of interpolator is used and how it has been modified to use in ROS.

[miaokeqin]

Yes, you can multiply the coefficient to adjust the speed. If the coefficient is greater than 1, it will decelerate. If it is less than 1, it will accelerate. The minimum value should not be less than 0.6, otherwise it will cause an overspeed alarm

The writing of this controller is not done by me, it is written by a colleague, I cannot provide a reference for the writing of the controller