Simulation velocity control, documentation and examples update (#109)

* Created interface class and put the RPC handlers in robot folder

* Now the robot functions interface work just like before

* Added templates and generation for simulated services

* Added the simulation services to the driver headers

* Added newly generated files to repo

* Added simulation option to the driver so the gazebo package can launch the kortex_driver too, in a different mode

* Adapt joint_limits files to correctly take prefix option

* Change description and control files to support prefix option

* Change MoveIt config launch and config files to enable prefix option - only SRDF modifications remain

* Added xacro usage in SRDF files to be able to prefix joint names

* Adjust ompl planning launch and config files to support prefixing joint names

* Remove --inorder from xacro calls since it's default now

* Set default gripper in launch files

* Add namespace to launch file parameters passed to driver

* Added class and implementation for simulator

* Add stubs and register handlers for Action API

* Add implementation for default actions and ReacAction/ReadAllActions

* Added some unit tests for simulator and CreateAction implentation

* Added the case where the prefix could be empty in XACRO

* Added prefix support for fake controllers in MoveIt configs

* Added a missing prefix value for Gen3 and fix the if and unless tags

* Add test launch file to launch simulator unit tests

* Don't create the gripper Move Group if no gripper on the arm

* Added DeleteAction implementation and tests

* Add implementation and tests for UpdateAction

* Added basic frame for a threaded action executor

* Added publishing for action topic before and after action execution

* Added StopAction logic and tests for executing and aborting actions

* Added stubs for other RPCs we want to support at first in simulation

* Added implementation for other RPCs worth simulating

* Added some data structure checks and TODOs in executors

* Added tests for newly added RPCs

* Remove merge artefacts

* Added implementation for ReachJointAngles action - optimal duration implementation remain

* Added KDL parsing and solvers construction

* Add max velocities and max accelerations to joint_limits.yaml files

* Add a helper to fill KortexError and finish velocity profiles implementation for ReachJointAngles

* Now use the velocity profile data to fill the trajectory

* Add implementation for angular position control

* Add same ROS parameters than real arm and adjust first timestamp to not be 0

* Fix MoveIt config Gen3 lite home position

* Added ReachPose implementation, twist limits still hardcoded

* Add twist limits files for simulation

* Use the correct Cartesian velocity and acceleration values for each arm and correctly use velocity limits in constraints

* Add simulated feedback support and change current state to fetch joint_states instead of controller state

* Added implementation for gripper control

* Fixed a Robotiq 2F-140 typo in the moveit config

* Added indexes for first arm joint and gripper joint because sometimes gripper joint is at the start of joint_states and sometimes it's at the end

* Changed the way the individual position controllers are loaded and started as well as added service client in simulator to switch controllers based on RPC called

* Added implementation for angular velocity control and added joint limits to simulation class. Will need a way to wrap-around angles now that velocity control for infinite spinning joint is on.

* Added wrapping around in reach joint angles

* Fix a problem with timestamps

* Added an implementation for SendTwistCommand but not stable enough to be activated so handler is commented out

* Dont start the joint_7 controller for a 6DOF robot

* Changed the examples to use notifications to check whether an action is finished or not and removed the sleeps

* Put back the homing script for Gazebo and removed the copyright notice since we don't use MoveIt anymore to home the arm

* Adapt documentation for new simulation features

* Updated the package.xml's with bumped versions and author and maintainer tags

kortex_control/package.xml

@@ -1,12 +1,12 @@
 <package>
   <name>kortex_control</name>
-  <version>2.2.0</version>
+  <version>2.2.2</version>
   <description>
-    <p>Gazebo Ros Control package for Kortex robots</p>
-    <p>This package contains launch and configuration files for the ros_control controllers for simulation</p>
+    <p>Gazebo ROS Control package for simulated Kortex robots</p>
+    <p>This package contains configuration files for the gazebo_ros_control controllers for simulation of Kortex arms</p>
   </description>
   <author email="[email protected]">Alexandre Vannobel</author>
-  <maintainer email="support@kinova.ca" />
+  <maintainer email="avannobel@kinova.ca" />
   <license>BSD</license>
   <buildtool_depend>catkin</buildtool_depend>
   <build_depend>roslaunch</build_depend>

kortex_description/package.xml

@@ -1,12 +1,13 @@
 <package>
   <name>kortex_description</name>
-  <version>2.2.1</version>
+  <version>2.2.2</version>
   <description>
-    <p>URDF Description package for Kortex-compatible robots</p>
+    <p>URDF and xacro description package for Kortex robots</p>
     <p>This package contains configuration data, 3D models and launch files
-for Kortex-compatible arms and grippers</p>
+for Kortex arms and supported grippers</p>
   </description>
-  <maintainer email="[email protected]" />
+  <author email="[email protected]">Alexandre Vannobel</author>
+  <maintainer email="[email protected]" />
   <license>BSD</license>
   <buildtool_depend>catkin</buildtool_depend>
   <build_depend>roslaunch</build_depend>

kortex_driver/CMakeLists.txt

@@ -17,7 +17,7 @@ if(NOT CMAKE_BUILD_TYPE)
 endif()
 
 ## find catkin and any catkin packages
-find_package(catkin REQUIRED COMPONENTS roscpp rospy std_msgs message_generation actionlib control_msgs urdf moveit_ros_planning_interface)
+find_package(catkin REQUIRED COMPONENTS roscpp rospy std_msgs controller_manager_msgs message_generation actionlib control_msgs urdf moveit_ros_planning_interface)
 find_package(Boost REQUIRED COMPONENTS system)
 
 file(GLOB_RECURSE generated_files RELATIVE ${PROJECT_SOURCE_DIR} "src/generated/robot/*.cpp" "src/generated/simulation/*.cpp")

kortex_driver/include/kortex_driver/non-generated/kortex_arm_simulation.h

@@ -15,10 +15,11 @@
 
 // ROS
 #include <ros/ros.h>
+#include <std_msgs/Empty.h>
 #include <control_msgs/JointTrajectoryControllerState.h>
 #include <control_msgs/FollowJointTrajectoryAction.h>
 #include <control_msgs/GripperCommandAction.h>
-#include <actionlib/client/simple_action_client.h>
+#include <actionlib/client/action_client.h>
 
 // MoveIt
 #include <moveit/move_group_interface/move_group_interface.h>
@@ -62,6 +63,12 @@
 #include "kortex_driver/SendGripperCommand.h"
 #include "kortex_driver/ApplyEmergencyStop.h"
 
+enum class ControllerType
+{
+  kTrajectory, // this is for the JointTrajectoryController
+  kIndividual  // this is for the individual JointPositionController's
+};
+
 class KortexArmSimulation
 {
   public:
@@ -98,10 +105,24 @@ class KortexArmSimulation
 
     // Publishers
     ros::Publisher m_pub_action_topic;
+    std::vector<ros::Publisher> m_pub_position_controllers;
 
     // Subscribers
     ros::Subscriber m_sub_joint_trajectory_controller_state;
     ros::Subscriber m_sub_joint_state;
+    ros::Subscriber m_sub_joint_speeds;
+    ros::Subscriber m_sub_twist;
+    ros::Subscriber m_sub_clear_faults;
+    ros::Subscriber m_sub_stop;
+    ros::Subscriber m_sub_emergency_stop;
+
+    // Service clients
+    ros::ServiceClient m_client_switch_controllers;
+    ControllerType m_active_controller_type;
+    std::vector<std::string> m_trajectory_controllers_list;
+    std::vector<std::string> m_position_controllers_list;
+    std::vector<double> m_velocity_commands;
+    kortex_driver::Twist m_twist_command;
 
     // Action clients
     std::unique_ptr<actionlib::SimpleActionClient<control_msgs::FollowJointTrajectoryAction>> m_follow_joint_trajectory_action_client;
@@ -114,6 +135,8 @@ class KortexArmSimulation
     // Arm information
     std::string m_arm_name;
     std::vector<std::string> m_arm_joint_names;
+    std::vector<double> m_arm_joint_limits_min;
+    std::vector<double> m_arm_joint_limits_max;
     std::vector<float> m_arm_velocity_max_limits;
     std::vector<float> m_arm_acceleration_max_limits;
     float m_max_cartesian_twist_linear;
@@ -148,14 +171,14 @@ class KortexArmSimulation
     // Threading
     std::atomic<bool> m_is_action_being_executed;
     std::atomic<bool> m_action_preempted;
+    std::atomic<int> m_current_action_type;
     std::thread m_action_executor_thread;
 
     // MoveIt-related
     std::unique_ptr<moveit::planning_interface::MoveGroupInterface> m_moveit_arm_interface;
     std::unique_ptr<moveit::planning_interface::MoveGroupInterface> m_moveit_gripper_interface;
 
     // Subscription callbacks and data structures with their mutexes
-    // void cb_joint_trajectory_controller_state(const control_msgs::JointTrajectoryControllerState& state);
     void cb_joint_states(const sensor_msgs::JointState& state);
     sensor_msgs::JointState m_current_state;
     bool m_first_state_received;
@@ -165,17 +188,23 @@ class KortexArmSimulation
     // Helper functions
     bool IsGripperPresent() const {return !m_gripper_name.empty();}
     void CreateDefaultActions();
-    kortex_driver::KortexError FillKortexError(uint32_t code, uint32_t subCode, const std::string& description = "") const;
-
-    // Executors
-    void JoinThreadAndCancelAction();
+    bool SwitchControllerType(ControllerType new_type);
     void PlayAction(const kortex_driver::Action& action);
+    void JoinThreadAndCancelAction();
+    kortex_driver::KortexError FillKortexError(uint32_t code, uint32_t subCode, const std::string& description = std::string()) const;
     kortex_driver::KortexError ExecuteReachJointAngles(const kortex_driver::Action& action);
     kortex_driver::KortexError ExecuteReachPose(const kortex_driver::Action& action);
     kortex_driver::KortexError ExecuteSendJointSpeeds(const kortex_driver::Action& action);
     kortex_driver::KortexError ExecuteSendTwist(const kortex_driver::Action& action);
     kortex_driver::KortexError ExecuteSendGripperCommand(const kortex_driver::Action& action);
     kortex_driver::KortexError ExecuteTimeDelay(const kortex_driver::Action& action);
+
+    // Callbacks
+    void new_joint_speeds_cb(const kortex_driver::Base_JointSpeeds& joint_speeds);
+    void new_twist_cb(const kortex_driver::TwistCommand& twist);
+    void clear_faults_cb(const std_msgs::Empty& empty);
+    void stop_cb(const std_msgs::Empty& empty);
+    void emergency_stop_cb(const std_msgs::Empty& empty);
 };
 
 #endif //_KORTEX_ARM_SIMULATION_H_

kortex_driver/include/kortex_driver/non-generated/kortex_math_util.h

@@ -12,6 +12,7 @@
  */
 
 #include <cmath>
+#include "kortex_driver/Twist.h"
 
 class KortexMathUtil
 {
@@ -21,10 +22,16 @@ class KortexMathUtil
 
     static double toRad(double degree);
     static double toDeg(double rad);
+    static int getNumberOfTurns(double rad_not_wrapped);
     static double wrapRadiansFromMinusPiToPi(double rad_not_wrapped);
+    static double wrapRadiansFromMinusPiToPi(double rad_not_wrapped, int& number_of_turns);
     static double wrapDegreesFromZeroTo360(double deg_not_wrapped);
+    static double wrapDegreesFromZeroTo360(double deg_not_wrapped, int& number_of_turns);
     static double relative_position_from_absolute(double absolute_position, double min_value, double max_value);
     static double absolute_position_from_relative(double relative_position, double min_value, double max_value);
+
+    // kortex_driver::Twist helper functions
+    static kortex_driver::Twist substractTwists(const kortex_driver::Twist& a, const kortex_driver::Twist& b);
 };
 
 #endif

kortex_driver/package.xml

@@ -1,10 +1,12 @@
 <?xml version="1.0"?>
 <package format="2">
   <name>kortex_driver</name>
-  <version>2.2.1</version>
-  <description>THe kortex package that act as a robot's driver.</description>
+  <version>2.2.2</version>
+  <description>The package that acts as a robot's driver. Supports simulated and real Kortex robots.</description>
 
-  <maintainer email="[email protected]" />
+  <author email="[email protected]">Hugo Lamontagne</author>
+  <author email="[email protected]">Alexandre Vannobel</author>
+  <maintainer email="[email protected]" />
 
   <license>BSD</license>
 
@@ -13,6 +15,7 @@
   <build_depend>rospy</build_depend>
   <build_depend>std_msgs</build_depend>
   <build_depend>control_msgs</build_depend>
+  <build_depend>controller_manager_msgs</build_depend>
   <build_depend>actionlib</build_depend>
   <build_depend>moveit_ros_planning_interface</build_depend>
   <build_depend>kdl_parser</build_depend>

kortex_driver/readme.md

@@ -51,7 +51,7 @@ The `kortex_driver` node is the node responsible for the communication between t
 - **arm** : Name of your robot arm model. See the `kortex_description/arms` folder to see the available robot models. The default value is **gen3**.
 - **dof** : Number of DOFs of your robot. The default value is for Gen3 is **7** and the default value for Gen3 lite is **6**. You will have to specify this value only if you have a Gen3 6DOF.
 - **vision** : Boolean value to indicate if your arm has a Vision Module. The default value is for Gen3 is **true** and the default value for Gen3 lite is **false**. You will have to specify this value only if you have a Gen3 6DOF without a Vision Module. This argument only affects the visual representation of the arm in RViz. 
-- **gripper** : Name of your robot arm's tool / gripper. See the `kortex_description/grippers` folder to see the available end effector models (or to add your own). The default value is **""**. For Gen3, you can also put **robotiq_2f_85**. For Gen3 lite, you need to put **gen3_lite_2f**.
+- **gripper** : Name of your robot arm's tool / gripper. See the `kortex_description/grippers` folder to see the available end effector models (or to add your own). The default value is **""**. For Gen3, you can also put **robotiq_2f_85** or **robotiq_2f_140**. For Gen3 lite, the default (and only) gripper is **gen3_lite_2f**.
 - **robot_name** : This is the namespace in which the driver will run. It defaults to **my_$(arg arm)** (so "my_gen3" for arm="gen3").
 - **prefix** : This is an optional prefix for all joint and link names in the kortex_description. It is used to allow differentiating between different arms in the same URDF. It defaults to **an empty string**. **Note** : Changing the prefix invalidates the MoveIt! configuration, and requires modifying said configuration, plus the .yaml files with harcoded joint names.
 - **ip_address** : The IP address of the robot you're connecting to. The default value is **192.168.1.10**.

kortex_driver/src/non-generated/driver/kortex_arm_driver.cpp

@@ -668,13 +668,15 @@ void KortexArmDriver::registerSimulationHandlers()
     base_services_simulation->StopActionHandler = std::bind(&KortexArmSimulation::StopAction, m_simulator.get(), std::placeholders::_1);
     // Other services
     base_services_simulation->PlayCartesianTrajectoryHandler = std::bind(&KortexArmSimulation::PlayCartesianTrajectory, m_simulator.get(), std::placeholders::_1);
-    base_services_simulation->SendTwistCommandHandler = std::bind(&KortexArmSimulation::SendTwistCommand, m_simulator.get(), std::placeholders::_1);
     base_services_simulation->PlayJointTrajectoryHandler = std::bind(&KortexArmSimulation::PlayJointTrajectory, m_simulator.get(), std::placeholders::_1);
     base_services_simulation->SendJointSpeedsCommandHandler = std::bind(&KortexArmSimulation::SendJointSpeedsCommand, m_simulator.get(), std::placeholders::_1);
     base_services_simulation->SendGripperCommandHandler = std::bind(&KortexArmSimulation::SendGripperCommand, m_simulator.get(), std::placeholders::_1);
     base_services_simulation->StopHandler = std::bind(&KortexArmSimulation::Stop, m_simulator.get(), std::placeholders::_1);
     base_services_simulation->ApplyEmergencyStopHandler = std::bind(&KortexArmSimulation::ApplyEmergencyStop, m_simulator.get(), std::placeholders::_1);
 
+    // Uncomment this SendTwistCommand handler to use the twist command simulation - not stable
+    // base_services_simulation->SendTwistCommandHandler = std::bind(&KortexArmSimulation::SendTwistCommand, m_simulator.get(), std::placeholders::_1);
+
     // Prospects
     //SendSelectedJointSpeedCommand
     //PlaySelectedJointTrajectory