working branch

idra-lab · Nov 25, 2024 · adb7f2d · adb7f2d
1 parent 6b63858
commit adb7f2d
Show file tree

Hide file tree

Showing 7 changed files with 58 additions and 29 deletions.
diff --git a/lbr_bringup/config/config.rviz b/lbr_bringup/config/config.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
diff --git a/lbr_bringup/launch/real.launch.py b/lbr_bringup/launch/real.launch.py
@@ -43,9 +43,9 @@ def launch_setup(context: LaunchContext) -> List[LaunchDescriptionEntity]:
     lbr_state_broadcaster = LBRROS2ControlMixin.node_controller_spawner(
         controller="lbr_state_broadcaster"
     )
-    # lbr_motion_control_handle = LBRROS2ControlMixin.node_controller_spawner(
-    #     controller="motion_control_handle"
-    # )
+    lbr_motion_control_handle = LBRROS2ControlMixin.node_controller_spawner(
+        controller="motion_control_handle"
+    )
     controller = LBRROS2ControlMixin.node_controller_spawner(
         controller=LaunchConfiguration("ctrl")
     )
@@ -57,8 +57,8 @@ def launch_setup(context: LaunchContext) -> List[LaunchDescriptionEntity]:
                 joint_state_broadcaster,
                 force_torque_broadcaster,
                 lbr_state_broadcaster,
-                controller,
                 # lbr_motion_control_handle,
+                controller,
             ],
         )
     )

diff --git a/lbr_demos/lbr_demos_cpp/src/torque_sine_overlay.cpp b/lbr_demos/lbr_demos_cpp/src/torque_sine_overlay.cpp
@@ -14,7 +14,7 @@
 #include "lbr_fri_ros2/utils.hpp"
 
 class TorqueSineOverlay {
-  static constexpr double amplitude_ = 15.;  // Nm
+  static constexpr double amplitude_ = 0.5;  // Nm
   static constexpr double frequency_ = 0.25; // Hz
 
 public:

diff --git a/lbr_description/urdf/iiwa14/iiwa14_description.xacro b/lbr_description/urdf/iiwa14/iiwa14_description.xacro
@@ -292,5 +292,14 @@
 
         <link name="link_ee">
         </link>
+
+        <joint name="joint_tool" type="fixed">
+            <parent link="link_ee" />
+            <child link="link_tool" />
+            <origin xyz="0 0 0.01" rpy="0 0 0" />
+        </joint>
+
+        <link name="link_tool">
+        </link>
     </xacro:macro>
-</robot>
+</robot>
diff --git a/lbr_fri_ros2/src/interfaces/torque_command.cpp b/lbr_fri_ros2/src/interfaces/torque_command.cpp
@@ -31,15 +31,17 @@ void TorqueCommandInterface::buffered_command_to_fri(fri_command_t_ref command,
   }
 
   // PID
-  if (!joint_position_pid_.is_initialized()) {
-    joint_position_pid_.initialize(pid_parameters_, state.sample_time);
-  }
-  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);
+  // if (!joint_position_pid_.is_initialized()) {
+  //   joint_position_pid_.initialize(pid_parameters_, state.sample_time);
+  // }
+  // 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.";

diff --git a/lbr_ros2_control/config/lbr_controllers.yaml b/lbr_ros2_control/config/lbr_controllers.yaml
@@ -2,7 +2,7 @@
 # managers regardless of their namespace. Usefull in multi-robot setups.
 /**/controller_manager:
   ros__parameters:
-    update_rate: 500
+    update_rate: 1000
 
     # ROS 2 control broadcasters
     joint_state_broadcaster:
@@ -84,7 +84,7 @@
     # 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_ee"
+    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
@@ -100,12 +100,12 @@
 
     # 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, 
+    # 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_ee"
+    compliance_ref_link: "link_tool"
 
     command_interfaces:
       - effort
@@ -122,16 +122,16 @@
       - A7
     delta_tau_max: 1.0 # Nm
     stiffness:  # w.r.t. compliance_ref_link coordinates
-        trans_x: 200.0
-        trans_y: 200.0
-        trans_z: 200.0
-        rot_x: 20.0
-        rot_y: 20.0
-        rot_z: 20.0
+        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_ee"
+    end_effector_link: "link_tool"
     robot_base_link: "link_0"
     ft_sensor_ref_link: "link_ee"
     joints:
@@ -145,7 +145,7 @@
 
 /**/gravity_compensation:
   ros__parameters:
-    end_effector_link: "link_ee"
+    end_effector_link: "link_tool"
     robot_base_link: "link_0"
     ft_sensor_ref_link: "link_ee"
     command_interfaces:
@@ -161,4 +161,4 @@
       - A5
       - A6
       - A7
-    kuka: true
+    kuka: true
diff --git a/lbr_ros2_control/src/lbr_torque_command_controller.cpp b/lbr_ros2_control/src/lbr_torque_command_controller.cpp
@@ -80,6 +80,14 @@ bool LBRTorqueCommandController::reference_command_interfaces_() {
       torque_command_interfaces_.emplace_back(std::ref(command_interface));
     }
   }
+  // Print command interfaces
+  RCLCPP_INFO(this->get_node()->get_logger(), "Command interfaces:");
+  for (const auto &joint_position_command_interface : joint_position_command_interfaces_) {
+    RCLCPP_INFO(this->get_node()->get_logger(), "  %s", joint_position_command_interface.get().get_name());
+  }
+  for (const auto &torque_command_interface : torque_command_interfaces_) {
+    RCLCPP_INFO(this->get_node()->get_logger(), "  %s", torque_command_interface.get().get_name());
+  }
   if (joint_position_command_interfaces_.size() != KUKA::FRI::LBRState::NUMBER_OF_JOINTS) {
     RCLCPP_ERROR(
         this->get_node()->get_logger(),