Feature/reconfigure cart compliance controller

[firesurfer]

Should not be merged like this!

PR for #127

This is currently a proof of concept which seems to work on my setup. It still generates quite a lot of errors which do not seem to matter to much apparently as the test itself works.

Test setup

For testing purpose I have cartesian_complianct_controller_bottom in my controller.yaml.
The default end_effector_link and compliance_ref_link is ur_bot/magnet_gripper/attach
For testing I changed both to gripper_bot/center

cartesian_compliance_controller_bottom:
  ros__parameters:
    end_effector_link: "ur_bot/magnet_gripper/attach"
    robot_base_link: "ur_bot/base_link"
    ft_sensor_ref_link: "ur_bot/tool0"
    compliance_ref_link: "ur_bot/magnet_gripper/attach"
    joints:
      - ur_bot/shoulder_pan_joint
      - ur_bot/shoulder_lift_joint
      - ur_bot/elbow_joint
      - ur_bot/wrist_1_joint
      - ur_bot/wrist_2_joint
      - ur_bot/wrist_3_joint

    # Choose: position or velocity.
    command_interfaces:
      - position
        #- velocity

    stiffness:  # w.r.t. compliance_ref_link
        trans_x: 8900.0
        trans_y: 900.0
        trans_z: 900.0
        rot_x: 40.0
        rot_y: 40.0
        rot_z: 40.0

    solver:
        error_scale: 0.68
        iterations: 2

    pd_gains:
        trans_x: {p: 0.011, d: 0.00}
        trans_y: {p: 0.011, d: 0.00}
        trans_z: {p: 0.011, d: 0.00}
        rot_x: {p: 0.3}
        rot_y: {p: 0.3}
        rot_z: {p: 0.3}

#Load controller
ros2 control load_controller --set-state active cartesian_compliance_controller_bottom

#Deactivate controller
ros2 control set_controller_state cartesian_compliance_controller_bottom inactive

#Perform parameter calls
ros2 param set /cartesian_compliance_controller_bottom end_effector_link gripper_bot/center
ros2 param set /cartesian_compliance_controller_bottom compliance_ref_link gripper_bot/center

#Reactivate controller
ros2 control set_controller_state cartesian_compliance_controller_bottom active 

#Then run a program which plans and execute a trajectory

The way I implemented this is a bit nasty:

1. I simply added a on_configure call in the on_activate method of the cart. compl. controller

rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn CartesianComplianceController::on_activate(
    const rclcpp_lifecycle::State & previous_state)
{

  on_configure(previous_state);

2. In the base class I removed the check if the base controller is already configured

rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn CartesianControllerBase::on_configure(
    const rclcpp_lifecycle::State & previous_state)
{
  /*if (m_configured)
  {
    return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS;
  }*/

3. In the ForwardDynamicsSolver I had to check if the link_mass parameter was already declared:

    // Set the initial value if provided at runtime, else use default value.
    if(!nh->has_parameter(m_params + "/link_mass"))
      m_min = nh->declare_parameter<double>(m_params + "/link_mass", 0.1);

Issues

[ros2_control_node-10] Warning: class_loader.ClassLoader: SEVERE WARNING!!! Attempting to unload library while objects created by this loader exist in the heap! You should delete your objects before attempting to unload the library or destroying the ClassLoader. The library will NOT be unloaded.
[ros2_control_node-10]          at line 127 in ./src/class_loader.cpp
[ros2_control_node-10] [INFO] [1685083683.473031148] [cartesian_compliance_controller_bottom]: Forward dynamics solver initialized
[ros2_control_node-10] [INFO] [1685083683.473061815] [cartesian_compliance_controller_bottom]: Forward dynamics solver has control over 6 joints
[ros2_control_node-10] Warning: class_loader.ClassLoader: SEVERE WARNING!!! Attempting to unload library while objects created by this loader exist in the heap! You should delete your objects before attempting to unload the library or destroying the ClassLoader. The library will NOT be unloaded.
[ros2_control_node-10]          at line 127 in ./src/class_loader.cpp
[ros2_control_node-10] [INFO] [1685083683.483277420] [cartesian_compliance_controller_bottom]: Forward dynamics solver initialized
[ros2_control_node-10] [INFO] [1685083683.483303519] [cartesian_compliance_controller_bottom]: Forward dynamics solver has control over 6 joints

I would guess this is somehow related to the multi inheritence used in the cart. compliance. controller. Nevertheless they should be fixed as they sound link a memory leak.

In order to do this properly I suggest:

1. m_min parameter should be passed to the FowardDynamicsSolver from the Caller instead of declaring it internally
2. In the CartesianControllerBase seperate general init calls from the once creating the kinematics chain and move the later into an extra method -> By avoiding to call on_configure again I think the warning from the ClassLoader should prob. also be solved.
3. In the cartesian compliance controller just reread the ref_link and reinit the kinematic chain. -> Just do the bare minimum as on_activate will always run in the RT loop of ros2control

But I won't starting on these points without any feedback from @stefanscherzinger

[firesurfer]

Just to keep moving with this. @stefanscherzinger do you have any feedback? Is this feature even desired or does it go against the way the cartesian controllers are designed?

[stefanscherzinger]

Is this feature even desired

Yes, I think that adds additional flexibility to the way people might use the compliance controller. If you need this, it's worth implementing! Unfortunately, I did not find the time so far to give detailed feedback. I'll be busy for the next two weeks and see that I can address this by then. In the meantime, you could follow your points to a proper solution.

[firesurfer]

@stefanscherzinger
I reworked the PR. It is now possible to load the controller, do something, deactivate the controller, set another
endeffector_link and compliance_ref_link and reactivate the controller.

Still missing:

1. Improved error handling. It is possible by specifying a bad link to bring a controller into an unrecoverable state
2. Improve the amount of code in on_activate
3. In general the "diamond" inheritance scheme made things way more difficult. I dont know if it would be easier to actually implement the cartesian compliance controller as seperate controller and accept to duplicate code.
4. I didnt do any testing with the other controllers so far apart from the cartesian motion controller which worked fine

EDIT: After some further testing I would even say inhering controllers is a bad idea. Point 1 actually originates from:
We activate the base, which works and claims the interfaces, then the child class does a check which fails -> Activation fails, but we still have claimed the interfaces

[firesurfer]

@stefanscherzinger just a quick reminder :)

[firesurfer]

@stefanscherzinger sorry for pushing again. But I would like to progress in this PR :)

[firesurfer]

@stefanscherzinger sorry to annoy you again ;)

[firesurfer]

@stefanscherzinger I think I found a way better solution:

I followed up on this issue today: ros-controls/ros2_control#1033 (comment)
And apparently it is possible to reconfigure a controller without unloading it. The only thing that needs to be changed is in the check if the controller was already configured:

cartesian_controllers/cartesian_controller_base/src/cartesian_controller_base.cpp

Line 141 in 4b5daab

if (m_configured)

[stefanscherzinger]

@firesurfer
Thanks for your patience and sorry for not responding earlier. I'm trying to pick up maintenance on this repo again. If still relevant for you, would you mind changing this PR to reflect your latest insights from here?

[firesurfer]

@stefanscherzinger I think this PR is still very relevant. But I need to see when I find some time to update it