Add Force-Gated Position and Velocity Controllers

[amalnanavati]

Description

As it stands, this package only implements a force-gated joint trajectory controller (specifically, a force-gated version of this controller). However, for MoveIt Servo to work well on the real robot, it needs to use a streaming controller, either a JointGroupPositionController or a JointGroupVelocityController. This PR adds force-gated versions of those two controllers.

Further, in order to have MoveIt's controller manager manage controllers not used by MoveGroup (i.e., the streaming controller used by MoveIt Servo), we needed to switch moveit_simple_controller_manager/MoveItSimpleControllerManager to moveit_ros_control_interface/Ros2ControlManager (done in ada_ros2#23). This further requires the force-gated joint trajectory controller to be wrapped in a class that inherits from moveit_ros_control_interface::ControllerHandleAllocator. This PR makes that change as well.

Design Decisions

The force-gated joint trajectory controller (JTC) was implemented by copying the code for the standard joint trajectory controller, and then adding the force-gating. This approach has the following downsides:

1. It becomes difficult to understand which parts of the code came from the base JTC, and which parts were modified due to force-gating.
2. It makes it challenging to pull in upstream changes.

Therefore, when implementing the force-gated position and velocity controllers, I instead inherit from the base controllers, and merely modify the functions that need to be for force-gating. In doing so, I define a parent class, ForceGateParent, that isolates the functions specific to force-gating. In theory, we should be able to force-gate any controller with few lines of code by using the ForceGateParent class and following the inheritance patterns in the force-gated position/velocity controllers.

Testing

Setup:

1. Pull ada_feeding#118 and ada_ros2#23 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:

• Using RVIZ, generate and execute a plan. Verify it successfully executes.
  • Sim
  • Real
• Using RVIZ, generate and execute a plan. Exert a force on the F/T sensor during execution. Verify that it fails.
  • Real
• Enable MoveIt Servo:
  • 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}}"
  • Toggle Servo On: ros2 service call /servo_node/start_servo std_srvs/srv/Trigger "{}"
• Run the keyboard teleop script: ros2 run ada_moveit ada_keyboard_teleop.py
  • Verify that the robot moves smoothly while the keys are pressed.
    • 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.)
    • Real
  • Verify that the robot stops immediately when the keys are released.
    • Sim (see NOTE above)
    • Real
  • While moving the robot, exert a force on the fork. Verify that the robot stops immeidately.
    • Real
• Disable MoveIt Servo: ros2 service call /servo_node/stop_servo std_srvs/srv/Trigger "{}"
• Using RVIZ, generate and execute a plan. Verify it successfully executes.
  • Sim
  • Real
• Using RVIZ, generate and execute a plan. Exert a force on the F/T sensor during execution. Verify that it fails.
  • Real

[amalnanavati]

@egordon, two discussion points:

1. As described in Design Decisions, I believe it is better to have the force-gated version of a controller inherit from, rather than copy the code of, the parent controller. I developed the position/velocity controllers that way, and am happy to modify the JTC to follow that convention as well. LMK what you think.
2. As it stands, the force-gating on the position/velocity controllers is on a per-message basis. Say servo keeps sending a particular velocity command. If the we exert a force on the F/T sensor, it will stop while the force is being exerted, but continue moving as soon as the force stops being exerted. This is a notable departure from the JTC, which terminates the whole trajectory when a force is exerted, thereby stopping all robot motion even after the force has stopped being exerted.
   1. I don’t think this is necessarily a problem, because this is a streaming controller and not a trajectory controller. Further, for feeding specifically, we have watchdog listener which will kill the controllers if the F/T sensor experiences a force.
   2. However, if we want to mirror the JTC functionality, we can implement a longer “stopped” period after experiencing a force. Specifically, we can add a parameter wrench_violation_extend_by_sec that takes in a number of seconds and only moves if that many secs have passed since the most recent wrench violation. In that way, after the F/T sensor experiences a force, it is guaranteed to be stopped for at least those many seconds. What do you think?

[egordon]

@amalnanavati

(1) This is 100% the better way to do it. The reason we didn't do it before is that, at the time, the humble version of the JTC had various bugs, e.g., ros-controls/ros2_controllers#671

Lmk if you want help with that the force-gate JTC.

(2) I agree that this is not necessarily a problem. Consider how servo vs. planning handles collisions: in planning, if there is a collision, planning fails. But with servo, if there is a collision, you can still control the robot, but just not in the direction of the collision object. I think this mirrors that behavior. I don't think (ii) is necessary, but if you want to add a parameter that doesn't affect older functionality, I'm not going to stop you.

[egordon]

I think this will be fine and is safe to merge since it is purely additive. Only comment I think should be addressed is the multi-subscriber.

[egordon]

Unnecessary comment?

[egordon]

Unnecessary comment?

[egordon]

Flagging this too

[egordon]

Newline

[egordon]

Newline

[egordon]

Force gate JTC makes the subscriber once in on_configure(). This will re-make it every time the parameters are read/updated. I would wrap this in an if wrench_subscriber_ != nullptr.

Recall that the topic is defined to be read-only in the parameters file, so you should never have to re-make the subscriber in the lifetime of the controller.

[egordon]

Maybe clarify how this affects the life-cycle here.

This transitions to on_error(), and receiving a new command transitions it back to running?

[amalnanavati]

The JTC bug has not yet been backported to humble. I made Issue #29 to move to an inheritance model once that is backported.