Implementation for angular position control, Cartesian position contr…

…ol and gripper position control (#105)

* 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

* Address PR comments

* Fix tool frame for 6DOF Gen3 arm

kortex_control/grippers/gen3_lite_2f/config/gripper_action_controller_parameters.yaml

@@ -5,7 +5,7 @@ $(arg prefix)gen3_lite_2f_gripper_controller:
 
 gazebo_ros_control:
   pid_gains:
-        $(arg prefix)right_finger_bottom_joint: {p: 0.1, i: 0.0, d: 0.0} 
+        $(arg prefix)right_finger_bottom_joint: {p: 10.0, i: 0.0, d: 0.0} 
         $(arg prefix)right_finger_tip_joint: {p: 1.0, i: 0.0, d: 0.0} 
         $(arg prefix)left_finger_bottom_joint: {p: 10.0, i: 0.0, d: 0.0} 
         $(arg prefix)left_finger_tip_joint: {p: 1.0, i: 0.0, d: 0.0}

kortex_description/arms/gen3/6dof/config/joint_limits.yaml

@@ -5,3 +5,17 @@ joint_names:
   - $(arg prefix)joint_4
   - $(arg prefix)joint_5
   - $(arg prefix)joint_6
+maximum_velocities:
+  - 0.8727
+  - 0.8727
+  - 0.8727
+  - 0.8727
+  - 0.8727
+  - 0.8727
+maximum_accelerations:
+  - 1.0
+  - 1.0
+  - 1.0
+  - 10.0
+  - 10.0
+  - 10.0

kortex_description/arms/gen3/6dof/config/twist_limits.yaml

@@ -0,0 +1,4 @@
+maximum_linear_velocity: 0.5
+maximum_angular_velocity: 1.74
+maximum_linear_acceleration: 0.4
+maximum_angular_acceleration: 0.4

kortex_description/arms/gen3/6dof/urdf/gen3_macro.xacro

@@ -232,7 +232,7 @@
     <joint
       name="${prefix}end_effector"
       type="fixed">
-      <origin xyz="0 0 -0.0615250000000001" rpy="3.14159265358979 1.09937075168372E-32 0" />
+      <origin xyz="0 0 -0.0615250000000001" rpy="3.14159265358979 1.09937075168372E-32 3.14159265358979" />
       <parent
         link="${prefix}bracelet_link" />
       <child

kortex_description/arms/gen3/7dof/config/joint_limits.yaml

@@ -5,4 +5,20 @@ joint_names:
   - $(arg prefix)joint_4
   - $(arg prefix)joint_5
   - $(arg prefix)joint_6
-  - $(arg prefix)joint_7
+  - $(arg prefix)joint_7
+maximum_velocities:
+  - 0.8727
+  - 0.8727
+  - 0.8727
+  - 0.8727
+  - 0.8727
+  - 0.8727
+  - 0.8727
+maximum_accelerations:
+  - 1.0
+  - 1.0
+  - 1.0
+  - 1.0
+  - 10.0
+  - 10.0
+  - 10.0

kortex_description/arms/gen3/7dof/config/twist_limits.yaml

@@ -0,0 +1,4 @@
+maximum_linear_velocity: 0.5
+maximum_angular_velocity: 1.74
+maximum_linear_acceleration: 0.4
+maximum_angular_acceleration: 0.4

kortex_description/arms/gen3_lite/6dof/config/joint_limits.yaml

@@ -5,3 +5,17 @@ joint_names:
   - $(arg prefix)joint_4
   - $(arg prefix)joint_5
   - $(arg prefix)joint_6
+maximum_velocities:
+  - 0.5
+  - 0.5
+  - 0.5
+  - 0.5
+  - 0.5
+  - 0.5
+maximum_accelerations:
+  - 1.0
+  - 0.5
+  - 0.4
+  - 1.0
+  - 10.0
+  - 10.0

kortex_description/arms/gen3_lite/6dof/config/twist_limits.yaml

@@ -0,0 +1,4 @@
+maximum_linear_velocity: 0.3
+maximum_angular_velocity: 0.85
+maximum_linear_acceleration: 0.4
+maximum_angular_acceleration: 0.4

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

@@ -163,6 +163,7 @@ class KortexArmDriver
     bool isGripperPresent();
     void setAngularTrajectorySoftLimitsToMax();
     void publishRobotFeedback();
+    void publishSimulationFeedback();
     void registerSimulationHandlers();
 };
 

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

@@ -13,15 +13,34 @@
 *
 */
 
+// ROS
 #include <ros/ros.h>
+#include <control_msgs/JointTrajectoryControllerState.h>
+#include <control_msgs/FollowJointTrajectoryAction.h>
+#include <control_msgs/GripperCommandAction.h>
+#include <actionlib/client/simple_action_client.h>
 
+// MoveIt
+#include <moveit/move_group_interface/move_group_interface.h>
+
+// KDL
+#include <kdl/chain.hpp>
+#include <kdl/chainfksolverpos_recursive.hpp>
+#include <kdl/chainiksolverpos_nr.hpp>
+#include <kdl/chainiksolvervel_pinv.hpp>
+#include <kdl/velocityprofile_trap.hpp>
+
+// Standard
 #include <unordered_map>
 #include <thread>
+#include <mutex>
 
+// Kortex
 #include "kortex_driver/non-generated/kortex_math_util.h"
 
 #include "kortex_driver/ActionType.h"
 #include "kortex_driver/KortexError.h"
+#include "kortex_driver/BaseCyclic_Feedback.h"
 
 #include "kortex_driver/CreateAction.h"
 #include "kortex_driver/ReadAction.h"
@@ -43,8 +62,6 @@
 #include "kortex_driver/SendGripperCommand.h"
 #include "kortex_driver/ApplyEmergencyStop.h"
 
-#include <moveit/move_group_interface/move_group_interface.h>
-
 class KortexArmSimulation
 {
   public:
@@ -54,6 +71,8 @@ class KortexArmSimulation
     std::unordered_map<uint32_t, kortex_driver::Action> GetActionsMap() const;
     int GetDOF() const {return m_degrees_of_freedom;}
 
+    kortex_driver::BaseCyclic_Feedback GetFeedback();
+
     // Handlers for simulated Kortex API functions
     // Actions API
     kortex_driver::CreateAction::Response CreateAction(const kortex_driver::CreateAction::Request& req);
@@ -74,31 +93,58 @@ class KortexArmSimulation
     kortex_driver::ApplyEmergencyStop::Response ApplyEmergencyStop(const kortex_driver::ApplyEmergencyStop::Request& req);
 
   private:
-
+    // ROS
     ros::NodeHandle m_node_handle;
 
     // Publishers
     ros::Publisher m_pub_action_topic;
 
+    // Subscribers
+    ros::Subscriber m_sub_joint_trajectory_controller_state;
+    ros::Subscriber m_sub_joint_state;
+
+    // Action clients
+    std::unique_ptr<actionlib::SimpleActionClient<control_msgs::FollowJointTrajectoryAction>> m_follow_joint_trajectory_action_client;
+    std::unique_ptr<actionlib::SimpleActionClient<control_msgs::GripperCommandAction>> m_gripper_action_client;
+
     // Namespacing and prefixing information
     std::string m_prefix;
     std::string m_robot_name;
 
-    // Arm and gripper information
+    // Arm information
     std::string m_arm_name;
     std::vector<std::string> m_arm_joint_names;
+    std::vector<float> m_arm_velocity_max_limits;
+    std::vector<float> m_arm_acceleration_max_limits;
+    float m_max_cartesian_twist_linear;
+    float m_max_cartesian_twist_angular;
+    float m_max_cartesian_acceleration_linear;
+    float m_max_cartesian_acceleration_angular;
+
+    // Gripper information
     std::string m_gripper_name;
     std::vector<std::string> m_gripper_joint_names;
     std::vector<float> m_gripper_joint_limits_max;
     std::vector<float> m_gripper_joint_limits_min;
     int m_degrees_of_freedom;
 
+    // The indexes of the first arm joint and of the gripper joint in the "joint_states" feedback
+    int m_first_arm_joint_index;
+    int m_gripper_joint_index;
+
     // Action-related
     std::unordered_map<uint32_t, kortex_driver::Action> m_map_actions;
 
     // Math utility
     KortexMathUtil m_math_util;
 
+    // KDL chain, solvers and motions
+    KDL::Chain m_chain;
+    std::unique_ptr<KDL::ChainFkSolverPos_recursive> m_fk_solver;
+    std::unique_ptr<KDL::ChainIkSolverPos_NR> m_ik_pos_solver;
+    std::unique_ptr<KDL::ChainIkSolverVel_pinv> m_ik_vel_solver;
+    std::vector<KDL::VelocityProfile_Trap> m_velocity_trap_profiles;
+
     // Threading
     std::atomic<bool> m_is_action_being_executed;
     std::atomic<bool> m_action_preempted;
@@ -108,12 +154,21 @@ class KortexArmSimulation
     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;
+    kortex_driver::BaseCyclic_Feedback m_feedback;
+    std::mutex m_state_mutex;
+
     // 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 CancelAction();
+    void JoinThreadAndCancelAction();
     void PlayAction(const kortex_driver::Action& action);
     kortex_driver::KortexError ExecuteReachJointAngles(const kortex_driver::Action& action);
     kortex_driver::KortexError ExecuteReachPose(const kortex_driver::Action& action);

kortex_driver/package.xml

@@ -15,6 +15,7 @@
   <build_depend>control_msgs</build_depend>
   <build_depend>actionlib</build_depend>
   <build_depend>moveit_ros_planning_interface</build_depend>
+  <build_depend>kdl_parser</build_depend>
   <build_export_depend>roscpp</build_export_depend>
   <build_export_depend>rospy</build_export_depend>
   <build_export_depend>std_msgs</build_export_depend>

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

@@ -50,6 +50,10 @@ KortexArmDriver::KortexArmDriver(ros::NodeHandle nh):   m_node_handle(nh),
     {
         m_publish_feedback_thread = std::thread(&KortexArmDriver::publishRobotFeedback, this);
     }
+    else
+    {
+        m_publish_feedback_thread = std::thread(&KortexArmDriver::publishSimulationFeedback, this);
+    }
 
     // If we get here and no error was thrown we started the node correctly
     ROS_INFO("%sThe Kortex driver has been initialized correctly!%s", GREEN_COLOR_CONSOLE, RESET_COLOR_CONSOLE);
@@ -639,6 +643,16 @@ void KortexArmDriver::publishRobotFeedback()
     }
 }
 
+void KortexArmDriver::publishSimulationFeedback()
+{
+    ros::Rate rate(m_cyclic_data_publish_rate);
+    while (m_node_is_running)
+    {
+        m_pub_base_feedback.publish(m_simulator->GetFeedback());
+        rate.sleep();
+    }
+}
+
 void KortexArmDriver::registerSimulationHandlers()
 {
     BaseSimulationServices* base_services_simulation = dynamic_cast<BaseSimulationServices*>(m_base_ros_services);