merge

lbr_bringup/config/hardware.rviz

@@ -9,6 +9,7 @@ Panels:
         - /RobotModel1
         - /RobotModel1/Description Topic1
         - /Wrench1
+        - /InteractiveMarkers1
       Splitter Ratio: 0.40760868787765503
     Tree Height: 724
   - Class: rviz_common/Selection
@@ -143,6 +144,15 @@ Visualization Manager:
       Torque Arrow Scale: 0.30000001192092896
       Torque Color: 170; 85; 255
       Value: true
+    - Class: rviz_default_plugins/InteractiveMarkers
+      Enable Transparency: true
+      Enabled: true
+      Interactive Markers Namespace: /lbr/motion_control_handle
+      Name: InteractiveMarkers
+      Show Axes: false
+      Show Descriptions: true
+      Show Visual Aids: false
+      Value: true
   Enabled: true
   Global Options:
     Background Color: 21; 21; 26
@@ -225,5 +235,5 @@ Window Geometry:
   Views:
     collapsed: false
   Width: 1850
-  X: 1080
-  Y: 450
+  X: 710
+  Y: 382

lbr_bringup/lbr_bringup/ros2_control.py

@@ -41,7 +41,7 @@ def arg_ctrl() -> DeclareLaunchArgument:
                 "gravity_compensation"
             ],
         )
-
+    
     @staticmethod
     def arg_sys_cfg_pkg() -> DeclareLaunchArgument:
         return DeclareLaunchArgument(
@@ -68,6 +68,7 @@ def arg_use_sim_time() -> DeclareLaunchArgument:
 
     @staticmethod
     def node_ros2_control(
+        robot_description: Dict[str, str],
         robot_name: Optional[Union[LaunchConfiguration, str]] = LaunchConfiguration(
             "robot_name", default="lbr"
         ),
@@ -83,7 +84,8 @@ def node_ros2_control(
             package="controller_manager",
             executable="ros2_control_node",
             parameters=[
-                {"use_sim_time": use_sim_time},
+                robot_description,
+                {"use_sim_time": False},
                 PathJoinSubstitution(
                     [
                         FindPackageShare(
@@ -101,7 +103,9 @@ def node_ros2_control(
             namespace=robot_name,
             remappings=[
                 ("~/robot_description", "robot_description"),
-                ('motion_control_handle/target_frame', 'cartesian_impedance_controller/target_frame'),
+                ("cartesian_impedance_controller/target_frame", "target_frame"),
+                ("cartesian_impedance_controller/target_wrench", "target_wrench"),
+                ("motion_control_handle/target_frame", "target_frame"),
             ],
             **kwargs,
         )

lbr_demos/lbr_demos_advanced_cpp/config/lbr_system_config.yaml

@@ -2,8 +2,8 @@
 hardware:
   fri_client_sdk: # the fri_client_sdk version is used to create the correct state interfaces lbr_system_interface.xacro
     major_version: 1
-    minor_version: 15
-  client_command_mode: position # the command mode specifies the user-sent commands. Available: [position, torque, wrench]
+    minor_version: 17
+  client_command_mode: torque # the command mode specifies the user-sent commands. Available: [position, torque, wrench]
   port_id: 30200 # port id for the UDP communication. Useful in multi-robot setups
   remote_host: INADDR_ANY # the expected robot IP address. INADDR_ANY will accept any incoming connection
   rt_prio: 80 # real-time priority for the control loop
@@ -20,7 +20,7 @@ hardware:
   #   Set tau > robot sample time for more smoothing on the raw torque measurements
   #   Set tau << robot sample time for more smoothing on the raw torque measurements
   measured_torque_tau: 0.4
-  open_loop: true # KUKA works the best in open_loop control mode
+  open_loop: false # KUKA works the best in open_loop control mode
 
 estimated_ft_sensor: # estimates the external force-torque from the external joint torque values
   enabled: true # whether to enable the force-torque estimation

lbr_demos/lbr_demos_cpp/src/torque_sine_overlay.cpp

@@ -71,4 +71,4 @@ int main(int argc, char **argv) {
   rclcpp::spin(node);
   rclcpp::shutdown();
   return 0;
-};
+};

lbr_description/gazebo/lbr_gazebo.xacro

@@ -9,6 +9,11 @@
                 <parameters>$(find lbr_description)/ros2_control/lbr_controllers.yaml</parameters>
                 <ros>
                     <namespace>/${robot_name}</namespace>
+                    <!-- remapping for controller manager inside Gazebo plugin -->
+                    <remapping>~/robot_description:=robot_description</remapping>
+                    <remapping>cartesian_impedance_controller/target_frame:=target_frame</remapping>
+                    <remapping>cartesian_impedance_controller/target_wrench:=target_wrench</remapping>
+                    <remapping>motion_control_handle/target_frame:=target_frame</remapping>
                 </ros>
             </plugin>
         </gazebo>

lbr_fri_ros2/src/interfaces/torque_command.cpp

@@ -30,15 +30,18 @@ void TorqueCommandInterface::buffered_command_to_fri(fri_command_t_ref command,
     throw std::runtime_error(err);
   }
 
-  // exponential smooth
-  // if (!joint_position_filter_.is_initialized()) {
-  //   joint_position_filter_.initialize(state.sample_time);
+  // PID
+  // if (!joint_position_pid_.is_initialized()) {
+  //   joint_position_pid_.initialize(pid_parameters_, state.sample_time);
   // }
-  // joint_position_filter_.compute(command_target_.joint_position, command_.joint_position);
-
+  // joint_position_pid_.compute(
+  //     command_target_.joint_position, state.measured_joint_position,
+  //     std::chrono::nanoseconds(static_cast<int64_t>(state.sample_time * 1.e9)),
+  //     command_.joint_position);
+
+  command_.torque = command_target_.torque;
   command_.joint_position = command_target_.joint_position;
-
-
+  // RCLCPP_INFO_STREAM(rclcpp::get_logger(LOGGER_NAME()), "Command: " << command_.torque.data()[0]<< " " << command_.torque.data()[1]<< " " << command_.torque.data()[2]<< " " << command_.torque.data()[3]<< " " << command_.torque.data()[4]<< " " << command_.torque.data()[5]<< " " << command_.torque.data()[6]);
   // validate
   if (!command_guard_->is_valid_command(command_, state)) {
     std::string err = "Invalid command.";

lbr_ros2_control/config/lbr_controllers.yaml

@@ -0,0 +1,164 @@
+# Use of /** so that the configurations hold for controller
+# managers regardless of their namespace. Usefull in multi-robot setups.
+/**/controller_manager:
+  ros__parameters:
+    update_rate: 1000
+
+    # ROS 2 control broadcasters
+    joint_state_broadcaster:
+      type: joint_state_broadcaster/JointStateBroadcaster
+
+    force_torque_broadcaster:
+      type: force_torque_sensor_broadcaster/ForceTorqueSensorBroadcaster
+
+    # LBR ROS 2 control broadcasters
+    lbr_state_broadcaster:
+      type: lbr_ros2_control/LBRStateBroadcaster
+
+    # ROS 2 control controllers
+    joint_trajectory_controller:
+      type: joint_trajectory_controller/JointTrajectoryController
+
+    forward_position_controller:
+      type: position_controllers/JointGroupPositionController
+
+    # LBR ROS 2 control controllers
+    lbr_joint_position_command_controller:
+      type: lbr_ros2_control/LBRJointPositionCommandController
+
+    lbr_torque_command_controller:
+      type: lbr_ros2_control/LBRTorqueCommandController
+
+    lbr_wrench_command_controller:
+      type: lbr_ros2_control/LBRWrenchCommandController
+
+    cartesian_impedance_controller:
+      type: cartesian_impedance_controller/CartesianImpedanceController
+
+    gravity_compensation:
+      type: gravity_compensation/GravityCompensation
+
+    motion_control_handle:
+      type: cartesian_controller_handles/MotionControlHandle
+
+/**/force_torque_broadcaster:
+  ros__parameters:
+    frame_id: lbr/link_ee # namespace: https://github.com/ros2/rviz/issues/1103
+    sensor_name: estimated_ft_sensor
+
+/**/joint_trajectory_controller:
+  ros__parameters:
+    joints:
+      - A1
+      - A2
+      - A3
+      - A4
+      - A5
+      - A6
+      - A7
+    command_interfaces:
+      - position
+    state_interfaces:
+      - position
+      - velocity
+    state_publish_rate: 50.0
+    action_monitor_rate: 20.0
+
+/**/forward_position_controller:
+  ros__parameters:
+    joints:
+      - A1
+      - A2
+      - A3
+      - A4
+      - A5
+      - A6
+      - A7
+    interface_name: position
+
+/**/cartesian_impedance_controller:
+  ros__parameters:
+
+    # This is the tip of the robot tool that you usually use for your task.
+    # For instance, it could be the drilling bit of a screwdriver or a grinding
+    # tool.  When you specify a target_wrench, i.e. some additional forces that
+    # your robot should apply to its environment, that target_wrench gets
+    # applied in this frame.
+    end_effector_link: "link_tool"
+
+    # This is usually the link directly before the first actuated joint.  All
+    # controllers will build a kinematic chain from this link up to
+    # end_effector_link.  It's also the reference frame for the superposition
+    # of error components in all controllers.
+    robot_base_link: "link_0"
+
+    # This is the URDF link of your sensor. Sensor signals are assumed to be
+    # given in this frame.  It's important that this link is located somewhere
+    # between end_effector_link and robot_base_link.  If that's not the case,
+    # the controllers won't initialize and will emit an error message.
+    ft_sensor_ref_link: "link_ee"
+
+    # This is the link that the robot feels compliant about. It does not need
+    # to coincide with the end_effector_link, but for many use cases, this
+    # configuration is handy. When working with a screwdriver, for instance,
+    # setting compliance_ref_link == end_effector_link makes it easy to specify
+    # downward pushing forces without generating unwanted offset moments.
+    # On the other hand, an application could benefit from yielding a little in
+    # the robot's wrist while drawing a line on a surface with a pen.
+    compliance_ref_link: "link_tool"
+
+    command_interfaces:
+      - effort
+    state_interfaces:
+      - position
+      - velocity
+    joints:
+      - A1
+      - A2
+      - A3
+      - A4
+      - A5
+      - A6
+      - A7
+    delta_tau_max: 1.0 # Nm
+    stiffness:  # w.r.t. compliance_ref_link coordinates
+        trans_x: 2800.0  # 1900.0
+        trans_y: 2800.0  # 1900.0
+        trans_z: 1500.0  # 700.0
+        rot_x: 180.0
+        rot_y: 180.0
+        rot_z: 180.0
+    kuka: true
+/**/motion_control_handle:
+  ros__parameters:
+    end_effector_link: "link_tool"
+    robot_base_link: "link_0"
+    ft_sensor_ref_link: "link_ee"
+    joints:
+      - A1
+      - A2
+      - A3
+      - A4
+      - A5
+      - A6
+      - A7
+
+/**/gravity_compensation:
+  ros__parameters:
+    end_effector_link: "link_tool"
+    robot_base_link: "link_0"
+    ft_sensor_ref_link: "link_ee"
+    command_interfaces:
+      - effort
+    state_interfaces:
+      - position
+      - velocity
+    joints:
+      - A1
+      - A2
+      - A3
+      - A4
+      - A5
+      - A6
+      - A7
+    kuka: true