Implementation for angular position control, Cartesian position control and gripper position control

[alexvannobel]

This includes implementation for

• ReachJointAngles actions
• ReachPose actions
• GripperCommand actions
• Stop, StopAction and ApplyEmergencyStop functions

The bulk of the implementation is in kortex_simulation.h/.cpp.
This heavily uses KDL for forward and inverse kinematics.

As for how this is done:

• ReachJointAngles: The goal is transformed in radians and wrapped. A velocity profile for each joint is created given its max velocity or the wanted duration. If no constraint is given, the minimal duration is calculated. A trajectory is then created and sent to ROS Control for execution.

• ReachPose: The starting frame is calculated (with FK) from the current joint angles. The ending frame is supplied in the Action. A Path_Line object is created for the path we want to follow, as well as a Trapezoidal Velocity profile for the length we have to go through and concatenated in a TrajectorySegment object. This object is used to fill a Joint Trajectory (with IK) that will be executed by the ROS Control Joint Trajectory Controller.

• GripperCommand : The gripper command is simply transformed in absolute value (values ranging between the min and max values in the gripper's URDF) from relative value (between 0 and 1) given in the Action. It is then sent to the GripperCommand ROS Control controller for execution.

• All the "Stop" functions : Those functions cancel all the goals on the Action Servers so the arm and gripper stop moving at once. The "Fault" state is not simulated although it would not be hard to add.

[huiwenzhang]

@alexvannobel Hi, I am trying to control the robot with moveit. As it has been reported that the output trajectory from moveit can't be executed by the robot (as discussed here #66), so I am trying to calculated a joint trajectory, and then execute the trajectory with customized controller. This PR implements the reach joint angles action, it seems to fit my purpose very well. Meanwhile, the official repo provides a pre_cpmputed_joint_trajectory_action_server, which seems to solve the problem as well. What's the difference between these two implementations? I dived into the code, it seems the pre_cpmputed_joint_trajectory_action_server execute the trajectory by calling a low-level method called PlayPreComputedJointTrajectory with input data structure proto_trajectory defined in this repo. Your code execute the trajectory by using ros follow_join_trajectory_action controller with a standard JointTrajectory input (

ros_kortex/kortex_driver/src/non-generated/driver/kortex_arm_simulation.cpp

Line 646 in ad3c266

kortex_driver::KortexError KortexArmSimulation::ExecuteReachJointAngles(const kortex_driver::Action& action)

). I am wandering whether the gen3 arm works in this way. After all, the moveit use the same follow joint trajectory controller as far as I know. Besides, I am also confused about how to compute a valid trajectory for gen3 arm? As the official documentation, the trajectory should be interploated by 1ms interval, your code use 0.01s interval as I am not wrong. In a word, as for my purpose, which way should I use?