Implement MoveIt Servo

[amalnanavati]

Description

This PR configures MoveIt Servo and its affiliated controllers.

Testing

Setup:

1. Pull pr_ros_controllers#28 and re-build your workspace.
2. Launch the force-torque sensor:
   1. Sim: ros2 run ada_feeding dummy_ft_sensor.py
   2. Real: ros2 run forque_sensor_hardware forque_sensor_hardware
3. Launch MoveIt:
   1. Sim:ros2 launch ada_moveit demo.launch.py sim:=mock
   2. Real: ros2 launch ada_moveit demo.launch.py
4. Re-tare the F/T sensor: ros2 service call /wireless_ft/set_bias std_srvs/srv/SetBool "{data: true}"

Testing:

• [x] Using RVIZ, generate and execute a plan. Verify it successfully executes.
  • [x] Sim
  • [x] Real
• [x] Enable MoveIt Servo:
  • [x] Switch Controllers: ros2 service call /controller_manager/switch_controller controller_manager_msgs/srv/SwitchController "{activate_controllers: [\"jaco_arm_servo_controller\"], deactivate_controllers: [\"jaco_arm_controller\"], start_controllers: [], stop_controllers: [], strictness: 0, start_asap: false, activate_asap: false, timeout: {sec: 0, nanosec: 0}}"
  • [x] Toggle Servo On: ros2 service call /servo_node/start_servo std_srvs/srv/Trigger "{}"
• [x] Run the keyboard teleop script: ros2 run ada_moveit ada_keyboard_teleop.py
  • [x] Verify that the robot moves smoothly while the keys are pressed.
    • [x] Cartesian Control Linear (also verify that the orientation doesn't change):
      • [x] Sim (NOTE: In order to see updates in sim, you need to switch the planning group back to hand and then toggle off and on the MotionPlanning element in RVIZ.)
      • [x] Real
    • [x] Cartesian Control Angular (also verify that the position doesn't change):
      • [x] Sim (see NOTE above)
      • [x] Real
    • [x] Joint Control:
      • [x] Sim (see NOTE above)
      • [x] Real
  • [x] Verify that the robot stops immediately when the keys are released.
    • [x] Sim (see NOTE above)
    • [x] Real
• [x] Disable MoveIt Servo: ros2 service call /servo_node/stop_servo std_srvs/srv/Trigger "{}"
• [x] Using RVIZ, generate and execute a plan. Verify it successfully executes.
  • [x] Sim
  • [x] Real
• [x] Using RVIZ, generate and execute a plan. Exert a force on the F/T sensor during execution. Verify that it fails.
  • [x] Real

Follow-Up PR

This PR suffers from getting stuck in singularities during cartesian control. At such times, it is possible to get it out of those singularities by using joint control, but that may not always be super intuitive. Using IK instead of the inverse jacobian -- and particularly pick_ik -- seems to be able to alleviate this issue. Another approach is to modify the MoveIt Servo source here to use dampened jacobians. There should be a follow-up PR to address that.

[amalnanavati]

@egordon this PR is ready to review. I'll make the IK investigation another PR so it isn't blocking your acquisition work. As of this PR, MoveIt Servo should work on both sim and real.