Support ros velocity control

[aaronsnoswell]

This PR completes #15, adding support for velocity control of the arms.

• Message types movo_msg/JacoAngularVelocityCmd6DOF.msg and movo_msg/JacoAngularVelocityCmd7DOF.msg added
• Removed references to 'teleop / autonomous' control modes in jaco_joint_controller.py and kinova_api_wrapper.py
• ROS topics /movo/right_arm/angular_vel_cmd and /movo/left_arm/angular_vel_cmd added
  • If messages are published to one of these topics >1hz, the appropriate SIArmController class will switch from it's current control mode to angular velocity control mode, bypassing the software PID control loop.
  • If messages cease, the controller pauses and reverts to angular position control
• The same approach has been used for the cartesian velocity control mode (re-used existing code that was used for teleop mode.
• Added placeholder for cartesian position control mode (not implemented yet, prints a ROS error if used)

Testing performed;

• Tested on the UQ MOVO robot
  • Angular position trajectory commands via JacoActionClient still work
  • Angular velocity commands now work (e.g. try rostopic pub /movo/left_arm/angular_vel_cmd movo_msgs/JacoAngularVelocityCmd6DOF '{header: auto, theta_wrist_3_joint: 45.0}' --rate=100)
  • Cartesian velocity commands work (e.g. try rostopic pub /movo/right_arm/cartesian_vel_cmd movo_msgs/JacoCartesianVelocityCmd '{header: auto, x: 0.1}' --rate=100 OR try using the Joystick in active arm and gripper teleoperation mode)

The only API surfaces changed are internal methods of SIArmController, and one or two public methods in KinovaAPI. To the best of my knowledge, the changed methods in KinovaAPI are only used in SIArmController, therefore these changes should not break any existing code.

As a side note, the code in SIArmController/KinovaAPI is not very good. I fixed at least one logic error in this PR (pausing the controller would NOT pause the gripper control loop - could have safety implications) and have commented the code a lot more so it is easier to read. Besides this, I have tried to clean the code up as best I could without changing things to drastically. I also tried to stick with the formatting conventions used in the files I touched (where possible - the formatting in the files wasn't even self-consistent, making this difficult).

NB: I haven't given any consideration to creating an equivalent API in Gazebo - I am not familiar with Gazebo, or the MOVO Gazebo code. Maybe Kinova can help out here?

[aaronsnoswell]

Another consideration here: In the current form, SIArmController arbitrarily defaults to / gives preference to angular position control vs. the other 3 control modes. I implemented it this way to match how the code previously worked. I'm not sure if this makes sense going forwards though.

One alternate approach would be to have SIArmController subscribe to a new topic, e.g. /movo/right_arm/control_mode / /movo/left_arm/control_mode that publishes an enum message indicating which control mode should currently be active. This would break existing code that uses SIArmController (only MovoArmJTAs as far as I know), and would hence require effort and agreement on Kinova's part if this was the desired approach going forwards.

Thank you,

[martine1406]

Thanks @aaronsnoswell for this work and suggestions. This is still a work in progress. The improvements you brought are certainly very welcome.

[tttor]

@martine1406 We would be most grateful if Kinova could help on joint vel ctrl in Gazebo, since, as we know, it will be dangerous to test algorithms that use joint vel ctrl directly in the real. Thank you.

[martine1406]

Hi @tttor

Thanks for the suggestion. May I ask a few more questions that will help us better plan the work:

What are the main reasons you find it dangerous to test your velocity control algorithms on the real robot? Are you worried of potential collisions between the different parts of the robot? Are you worried you'll be sending wrong velocity commands?

Also, can you confirm your only want to send the arms velocity commands?

Finally, will you be using angular velocity commands only? Or Cartesian velocity commands as well? The main challenge with Cartesian velocity commands will be to replicate the inverse kinematics computation that is performed in the real robot. Implementing an angular velocity controller for the gazebo robot will probably be more easily achievable.

The real arms are transforming your velocity commands in small position increments. Every 10 ms, a new velocity command is processed. Sticking with angular velocity commands, I think that one approach here would be to program a code that listens to the velocity commands your controller outputs, then buffers these commands until 2000 points (that's the trajectory buffer size in the real Jaco2 arms). In that same code, you could have a timer that would look at the buffer every 10 ms and, if it finds a command, transforms that command in a small angular position and sends it to the Gazebo robot position controllers. That is, I think, what would represent the best the behaviour in the real Jaco2 robot.

I'll discuss your suggestion with the team in our next meeting. Let me know your answers to my questions at the beginning of this post Cheers Martine

[tttor]

Hi @martine1406

What are the main reasons you find it dangerous to test your velocity control algorithms on the real robot? Are you worried of potential collisions between the different parts of the robot? Are you worried you'll be sending wrong velocity commands?

We worry about all that you said :) And, as we know, it is always best to do sanity check in simulator first before doing it in real.

Also, can you confirm your only want to send the arms velocity commands?

(At least for now), we want to send joint velocity command to the robot arms

Finally, will you be using angular velocity commands only? Or Cartesian velocity commands as well?

(At least for now), we need angular velocity commands for the arm joints.

Thank you for your time and attention. Regards, tor