Recommendation for using Kortex API with MoveIt for collision avoidance?

[autonomousTurtle]

ROS1 Noetic, Gen3 Arm

I am looking to use velocity based control on my Gen 3 for visual servoing. In MoveIt, I know the Realtime servo package exists, but requires a velocity based controller, not a trajectory based controller which is all that is included with the kortex moveit package. I am also using the MoveIt to make sure the robot doesn't collide with itself or its environment.

I see that that a velocity based controller is included with the kortex_arm_driver, but as far as I am able to tell, it is unable to be integrated with MoveIt. The answer may be very simple and is just something I am unaware of, but is there a recommendation for how to perform velocity based control of a Kinova Gen3 arm while still getting the benefits of the MoveIt collision avoidance?

Thank you!

[felixmaisonneuve]

Hi @autonomousTurtle,

There is currently no velocity based controller in the ros driver, but this is something that you could implement if you are familiar with ROS and coding in general.

When you execute a trajectory using MoveIt, it sends a FollowJointTrajectoryAction that is catched by our joint_trajectory_action_server.cpp

The Joint trajectory is then read and converted in waypoints that are sent using our Gen3 Kortex API. All of this is done in the goal_received_callback function https://github.com/Kinovarobotics/ros_kortex/blob/6f9a44568eccb40048494a8b86b0dee7aec8c3fb/kortex_driver/src/non-generated/driver/joint_trajectory_action_server.cpp#L72

It is using the positions from the JointTrajectory https://github.com/Kinovarobotics/ros_kortex/blob/6f9a44568eccb40048494a8b86b0dee7aec8c3fb/kortex_driver/src/non-generated/driver/joint_trajectory_action_server.cpp#L140-L144

I do not know how MoveIt Realtime Servo Package works, but you could create your own velocity based action server in a similar way (by bindind callbacks from Realtime Servo Package to a function that sends veocity based commands via the arm's API).

You could use the SendJointSpeedsCommand and SendTwistCommand from the base module. You would call them like this : https://github.com/Kinovarobotics/ros_kortex/blob/6f9a44568eccb40048494a8b86b0dee7aec8c3fb/kortex_driver/src/non-generated/driver/joint_trajectory_action_server.cpp#L205

If MoveIt Realtime Servo Package does not send callback and it publishes the required velocity on a topic, you could add a subscriber to this topic that update the velocity of the arm. Our trajectory controller for our Jaco2 ros stack is implemented in such way : https://github.com/Kinovarobotics/kinova-ros/blob/melodic-devel/kinova_driver/src/joint_trajectory_action/joint_trajectory_action_server.cpp

Hope this helps, Felix

[autonomousTurtle]

Thanks Felix - I have spent some time digging in to your suggestions have come up with the following:

• MoveIt servo subscribes to /joint_state and publishes new joint position commands to a joint_group_position controller. As this robot does not have that, I created a translation layer to subscribe to the group position topic and publish to each joint controller individually. It seems to work alright - code below, if you have any suggestions I'd be open to them as well.

I seem to still have a few issues with the gazebo package:

1. When I start the gazebo simulation with $ roslaunch kortex_gazebo spawn_kortex_robot.launch , the robot arm starts at all 0 and then collapses to the ground and moves to the home position after the home_robot.py script start up. In the launch file, it designates the start position of the robot in gazebo with a few TODOs%22/%3E%20%3C!%2D%2DTODO%2D%2D%3E,-%3C!%2D%2D%20Load%20controller) - is the behavior I am observing expected or is it supposed to start at the given locations? I seem to be having issues with the servo package finding the starting position of the robot, and I think this may be it.

2. is there a recommended way to implement the twist_command in gazebo? It is clearly commented out in the driver script - line 714%3B). Do you have a recommendation for how to implement SendTwistCommand in gazebo besides uncommenting this and hoping for the best?

Translation layer between MoveIt Servo published topic /my_gen3/servo_command and the individual joint position controllers for the Kinova robot

servo_to_joint_translate.cpp

#include <iostream>
#include <ros/ros.h>
#include <geometry_msgs/Twist.h>
#include <std_srvs/Empty.h>
#include <kortex_driver/Base_JointSpeeds.h>
#include <kortex_driver/JointSpeed.h>
#include <std_msgs/Float64MultiArray.h>
#include <std_msgs/Float64.h>
#include <angles/angles.h>

using namespace std;

class ServoJointTranslate
{

private:
    // ROS Services
    ros::ServiceServer reset_service;

    // ROS Publishers
    ros::Publisher j1_pub;
    ros::Publisher j2_pub;
    ros::Publisher j3_pub;
    ros::Publisher j4_pub;
    ros::Publisher j5_pub;
    ros::Publisher j6_pub;
    ros::Publisher j7_pub;

    // ROS Subscriber
    ros::Subscriber vel_command_subscriber;

    // ROS Messages
    geometry_msgs::Twist vel_msg;

public:
    ServoJointTranslate(ros::NodeHandle *nh)
    {
        // my_service = nh_.advertiseService("/move_bb8_in_circle", &MoveBB8::my_callback, this);
        // ROS_INFO("The Service /move_bb8_in_circle is READY");
        ROS_INFO("Advertising vel");

        j1_pub = nh->advertise<std_msgs::Float64>("/my_gen3/joint_1_position_controller/command", 10);
        j2_pub = nh->advertise<std_msgs::Float64>("/my_gen3/joint_2_position_controller/command", 10);
        j3_pub = nh->advertise<std_msgs::Float64>("/my_gen3/joint_3_position_controller/command", 10);
        j4_pub = nh->advertise<std_msgs::Float64>("/my_gen3/joint_4_position_controller/command", 10);
        j5_pub = nh->advertise<std_msgs::Float64>("/my_gen3/joint_5_position_controller/command", 10);
        j6_pub = nh->advertise<std_msgs::Float64>("/my_gen3/joint_6_position_controller/command", 10);
        j7_pub = nh->advertise<std_msgs::Float64>("/my_gen3/joint_7_position_controller/command", 10);

        vel_command_subscriber = nh->subscribe("/my_gen3/servo_command", 1000, &ServoJointTranslate::callback, this);

        /*reset_service = nh->advertiseService("/reset_counter",
            &Translate::callback_reset, this); */
    }

    //"{joint_speeds : [{joint_identifier: 0, value: 10},{joint_identifier: 1, value: 0.0},{joint_identifier: 2, value: 0.0},{joint_identifier: 3, value: 0.0},{joint_identifier: 4, value: 0.0},{joint_identifier: 5, value: 0.0},{joint_identifier: 6, value: 0.0}], duration: 1}"
    void callback(const std_msgs::Float64MultiArray &msg)
    {
        //ROS_INFO_STREAM("incoming msg" << msg.data);

        std_msgs::Float64 j1_msg;
        j1_msg.data = angles::from_degrees(msg.data[0]);

        std_msgs::Float64 j2_msg;
        j2_msg.data = angles::from_degrees(msg.data[1]);

        std_msgs::Float64 j3_msg;
        j3_msg.data = angles::from_degrees(msg.data[2]);

        std_msgs::Float64 j4_msg;
        j4_msg.data = angles::from_degrees(msg.data[3]);

        std_msgs::Float64 j5_msg;
        j5_msg.data = angles::from_degrees(msg.data[4]);

        std_msgs::Float64 j6_msg;
        j6_msg.data = angles::from_degrees(msg.data[5]);

        std_msgs::Float64 j7_msg;
        j7_msg.data = angles::from_degrees(msg.data[6]);

        //j1_msg.data = 0;
        //j2_msg.data = 0;
        //j3_msg.data = 0;
        //j4_msg.data = 1.57;
        //j5_msg.data = 1.57;
        //j6_msg.data = 0;
        //j7_msg.data = 0;

        j1_pub.publish(j1_msg);
        j2_pub.publish(j2_msg);
        j3_pub.publish(j3_msg);
        j4_pub.publish(j4_msg);
        j5_pub.publish(j5_msg);
        j6_pub.publish(j6_msg);
        j7_pub.publish(j7_msg); 

        //ROS_INFO_STREAM("new message: " << j0_msg);

    }
};

int main(int argc, char **argv)
{
    ros::init(argc, argv, "servo_Joint_translate_node");
    ros::NodeHandle nh;

    ServoJointTranslate servoJointTranslate(&nh);

    ros::spin();

    return 0;
}

Thank you!

[felixmaisonneuve]

1. Gazebo is normally launched with paused physics exactly to avoid the arm to fall on the ground. From the readme page

   The package also uses a Python script to home the robot after the robot has been spawned. Gazebo is launched with paused physics. Otherwise, the arm would fall to the ground because the controllers are not fully loaded when the robot is spawned in the simulator. When everything is well loaded, the script unpauses Gazebo's physics and executes the robot's Home action.

So unless you manually removed the pause in the simulation, this is not normal and you might have an issue

2. I think uncommenting the line you mentionned is the best way to make Twist commands work. It says it is unstable probably because the algorithm implemented for the simulation (implemented in the ExecuteSendTwist function) is not as robust as the one implemented in an actual arm. If you try it and you have issues, you could always look at the implementatio and adpat it to your use case. I do not know what is unstable or not.

You code looks good. If it works for your use case, that's perfect.

[felixmaisonneuve]

Hi @autonomousTurtle

Do you still need help about this issue? Did you manage to manage to make your system work in a simulation?

I will close this issue in a couple weeks if I do not get an answer from you

Regards, Felix

[autonomousTurtle]

Thanks Felix - we are still working on the simulation. When we started it up, the robot arm did not follow the commanded joint positions, which may be either be jerk or physics related. We can close now if it is alright to re-open if we make progress and get stuck at a future point?

[felixmaisonneuve]

Good, I will close the issue then.

Please re-open it or open a new one if you make progress and you still need support

Best Regards, Felix