feat: better handling of RMW using environment variables + use any camera docker file

Author: danielsanjosepro

.devcontainer/docker-compose.yaml

@@ -2,10 +2,11 @@ services:
   devcontainer:
     image: crisp_controllers_demos:base
     build:
-      context: ..
+      context: ../docker
+      dockerfile: Dockerfile.robots
       args:
         ROS_DISTRO: humble
-      target: base
+      target: kinova
     container_name: crisp_controllers_demos_devcontainer
 
     # === Tune this to your application ===

.env.example

@@ -0,0 +1,14 @@
+# Robot configuration
+FRANKA_FAKE_HARDWARE=false
+ROBOT_IP=172.16.0.2
+LEFT_ROBOT_IP=172.16.1.2
+RIGHT_ROBOT_IP=172.16.0.2
+
+# Camera configuration
+CAMERA_NAME=camera
+CAMERA_NAMESPACE=camera
+
+# Middleware configuration
+# RMW=cyclone
+# RMW=zenoh
+# RMW=fastdd

.github/workflows/docker_build.yml

@@ -12,4 +12,4 @@ jobs:
     steps:
     - uses: actions/checkout@v4
     - name: Build the Docker image
-      run: docker build . --file Dockerfile
+      run: docker build . --file docker/Dockerfile.robots

.github/workflows/docker_build_cameras.yml

@@ -12,4 +12,4 @@ jobs:
     steps:
     - uses: actions/checkout@v4
     - name: Build the Docker image
-      run: docker build . --file Dockerfile.cameras
+      run: docker build . --file docker/Dockerfile.cameras

cyclone_config.xml config/cyclone_config.xmlcyclone_config.xml renamed to config/cyclone_config.xml

@@ -3,7 +3,7 @@
     <General>
       <AllowMulticast>true</AllowMulticast>
       <Interfaces>
-        <NetworkInterface name="enx607d0937fb24" />
+        <NetworkInterface name="${ROS_NETWORK_INTERFACE}" />
       </Interfaces>
     </General>
     <Discovery>

crisp_controllers

@@ -176,3 +176,22 @@
       use_coriolis_compensation: true
       use_local_jacobian: true
 
+  torque_feedback_controller:
+    ros__parameters:
+      joints:
+        - fr3_joint1
+        - fr3_joint2
+        - fr3_joint3
+        - fr3_joint4
+        - fr3_joint5
+        - fr3_joint6
+        - fr3_joint7
+      
+      end_effector_frame: fr3_hand_tcp
+      base_frame: base
+      
+      k_fb: 0.6
+      kd: 0.1
+      joint_source_topic: target_joint
+      torque_threshold: 0.1
+

crisp_controllers_robot_demos/config/fr3/controllers.yaml

@@ -21,6 +21,12 @@ left:
       cartesian_impedance_controller:
         type: crisp_controllers/CartesianController
 
+      torque_feedback_controller:
+        type: crisp_controllers/TorqueFeedbackController
+
+      external_torques_broadcaster:
+        type: franka_force_feedback_controllers/ExternalTorquesBroadcaster
+
   joint_trajectory_controller:
     ros__parameters:
       command_interfaces:
@@ -174,3 +180,31 @@ left:
       use_local_jacobian: true
       limit_error: true
       limit_torques: true
+
+  torque_feedback_controller:
+    ros__parameters:
+      joints:
+        - left_fr3_joint1
+        - left_fr3_joint2
+        - left_fr3_joint3
+        - left_fr3_joint4
+        - left_fr3_joint5
+        - left_fr3_joint6
+        - left_fr3_joint7
+      end_effector_frame: left_fr3_hand_tcp
+      base_frame: left_base
+      k_fb: 0.9
+      kd: 0.1
+      joint_source_topic: /right/external_torques_joint_state
+      torque_threshold: 2.0
+
+  external_torques_broadcaster:
+    ros__parameters:
+      joints:
+        - left_fr3_joint1
+        - left_fr3_joint2
+        - left_fr3_joint3
+        - left_fr3_joint4
+        - left_fr3_joint5
+        - left_fr3_joint6
+        - left_fr3_joint7

crisp_controllers_robot_demos/config/fr3/left_controllers.yaml

@@ -21,6 +21,12 @@ right:
       joint_impedance_controller:
         type: crisp_controllers/CartesianController
 
+      torque_feedback_controller:
+        type: crisp_controllers/TorqueFeedbackController
+
+      external_torques_broadcaster:
+        type: franka_force_feedback_controllers/ExternalTorquesBroadcaster
+
   joint_trajectory_controller:
     ros__parameters:
       command_interfaces:
@@ -174,3 +180,31 @@ right:
       use_local_jacobian: true
       limit_error: true
       limit_torques: true
+
+  torque_feedback_controller:
+    ros__parameters:
+      joints:
+        - right_fr3_joint1
+        - right_fr3_joint2
+        - right_fr3_joint3
+        - right_fr3_joint4
+        - right_fr3_joint5
+        - right_fr3_joint6
+        - right_fr3_joint7
+      end_effector_frame: right_fr3_hand_tcp
+      base_frame: right_base
+      k_fb: 0.9
+      kd: 0.1
+      joint_source_topic: /left/external_torques_joint_state
+      torque_threshold: 2.0
+
+  external_torques_broadcaster:
+    ros__parameters:
+      joints:
+        - right_fr3_joint1
+        - right_fr3_joint2
+        - right_fr3_joint3
+        - right_fr3_joint4
+        - right_fr3_joint5
+        - right_fr3_joint6
+        - right_fr3_joint7

crisp_controllers_robot_demos/config/fr3/right_controllers.yaml

@@ -31,13 +31,13 @@
         - joint_a6
         - joint_a7
       gains:
-        fr3_joint1: { p: 600., d: 30., i: 0., i_clamp: 1. }
-        fr3_joint2: { p: 600., d: 30., i: 0., i_clamp: 1. }
-        fr3_joint3: { p: 600., d: 30., i: 0., i_clamp: 1. }
-        fr3_joint4: { p: 600., d: 30., i: 0., i_clamp: 1. }
-        fr3_joint5: { p: 250., d: 10., i: 0., i_clamp: 1. }
-        fr3_joint6: { p: 150., d: 10., i: 0., i_clamp: 1. }
-        fr3_joint7: { p: 50., d: 5., i: 0., i_clamp: 1. }
+        joint_a1: { p: 1000., d: 30., i: 0., i_clamp: 1. }
+        joint_a2: { p: 1000., d: 30., i: 0., i_clamp: 1. }
+        joint_a3: { p: 1000., d: 30., i: 0., i_clamp: 1. }
+        joint_a4: { p: 1000., d: 30., i: 0., i_clamp: 1. }
+        joint_a5: { p: 1000., d: 10., i: 0., i_clamp: 1. }
+        joint_a6: { p: 1000., d: 10., i: 0., i_clamp: 1. }
+        joint_a7: { p: 1000., d: 5., i: 0., i_clamp: 1. }
 
   joint_state_broadcaster:
     ros__parameters: