Merge branch 'foxy' into dev-foxy-optas

README.md

@@ -14,7 +14,7 @@ mkdir -p lbr_fri_ros2_stack_ws/src && cd lbr_fri_ros2_stack_ws
 wget https://raw.githubusercontent.com/KCL-BMEIS/lbr_fri_ros2_stack/foxy/lbr_fri_ros2_stack/repos.yml -P src
 vcs import src < src/repos.yml
 rosdep install --from-paths src --ignore-src -r -y
-colcon build
+colcon build --cmake-args -DCMAKE_BUILD_TYPE=Release
 ```
 Next, launch the simulation via
 ```shell

lbr_bringup/launch/lbr_bringup.launch.py

@@ -30,16 +30,6 @@ def launch_setup(context, *args, **kwargs):
         )
     }
 
-    # Load LBR FRI ROS2
-    lbr_app_node = Node(
-        package="lbr_fri_ros2",
-        executable="lbr_app",
-        emulate_tty=True,
-        output="screen",
-        parameters=[robot_description],
-        condition=UnlessCondition(LaunchConfiguration("sim")),
-    )
-
     # Load controls
     control = IncludeLaunchDescription(
         PythonLaunchDescriptionSource(
@@ -95,7 +85,7 @@ def launch_setup(context, *args, **kwargs):
         ],
     )
 
-    return [lbr_app_node, simulation, control, move_group]
+    return [simulation, control, move_group]
 
 
 def generate_launch_description():

lbr_demos/lbr_fri_ros2_advanced_python_demos/doc/lbr_fri_ros2_advanced_python_demos.rst

@@ -24,6 +24,6 @@ Admittance Controller
 
 .. code-block:: bash
 
-    ros2 run lbr_fri_ros2_advanced_python_demos admittance_control_node
+    ros2 launch lbr_fri_ros2_advanced_python_demos admittance_control_node.launch.py model:=iiwa7 # [iiwa7, iiwa14, med7, med14]
 
 4. Now gently move the robot at the end-effector.

lbr_demos/lbr_fri_ros2_advanced_python_demos/launch/admittance_control_node.launch.py

@@ -0,0 +1,43 @@
+import os
+
+import xacro
+from ament_index_python import get_package_share_directory
+from launch import LaunchContext, LaunchDescription
+from launch.actions import DeclareLaunchArgument, OpaqueFunction
+from launch.substitutions import LaunchConfiguration
+from launch_ros.actions import Node
+
+
+def launch_setup(context: LaunchContext) -> LaunchDescription:
+    model = LaunchConfiguration("model").perform(context)
+
+    robot_description = {
+        "robot_description": xacro.process(
+            os.path.join(
+                get_package_share_directory("lbr_description"),
+                "urdf",
+                model,
+                f"{model}.urdf.xacro",
+            ),
+            mappings={"sim": "false"},
+        )
+    }
+
+    admittance_control_node = Node(
+        package="lbr_fri_ros2_advanced_python_demos",
+        executable="admittance_control_node",
+        output="screen",
+        parameters=[robot_description],
+    )
+
+    return [admittance_control_node]
+
+
+def generate_launch_description() -> LaunchDescription:
+    model_arg = DeclareLaunchArgument(
+        name="model",
+        default_value="iiwa7",
+        description="The LBR model in use.",
+        choices=["iiwa7", "iiwa14", "med7", "med14"],
+    )
+    return LaunchDescription([model_arg, OpaqueFunction(function=launch_setup)])

lbr_demos/lbr_fri_ros2_advanced_python_demos/lbr_fri_ros2_advanced_python_demos/admittance_control_node.py

@@ -1,144 +1,74 @@
-import os
-
-import optas
 import numpy as np
 import rclpy
-import xacro
-from ament_index_python import get_package_share_directory
-from rclpy.qos import qos_profile_sensor_data
+from rclpy.duration import Duration
 from rclpy.node import Node
+from rclpy.qos import QoSProfile, ReliabilityPolicy
 
 from lbr_fri_msgs.msg import LBRCommand, LBRState
 
-
-class Controller(object):
-    def __init__(
-        self,
-        urdf_string: str,
-        end_link_name: str = "lbr_link_ee",
-        root_link_name: str = "lbr_link_0",
-        f_threshold: np.ndarray = np.array([6.0, 6.0, 6.0, 1.0, 1.0, 1.0]),
-        dq_gain: np.ndarray = np.array([0.2, 0.2, 0.2, 0.2, 0.2, 0.2, 0.2]),
-        dx_gain: np.ndarray = np.array([1.0, 1.0, 1.0, 20.0, 40.0, 60.0]),
-        smooth: float = 0.02,
-    ) -> None:
-        robot = optas.RobotModel(urdf_string=urdf_string)
-        J = robot.get_global_link_geometric_jacobian_function(end_link_name)
-        self.jacobian = lambda q: J(
-            q
-        ).toarray()  # toarray: convert casadi.DM -> numpy.ndarray
-
-        self.f_threshold_ = f_threshold
-        self.dq_gain_ = np.diag(dq_gain)
-        self.dx_gain_ = np.diag(dx_gain)
-        self.smooth_ = smooth
-
-        self.dof_ = robot.ndof
-        self.dq_ = np.zeros(self.dof_)
-
-    def __call__(self, q: np.ndarray, tau_ext: np.ndarray) -> np.ndarray:
-        jacobian = self.jacobian(q)
-        # J^T fext = tau
-        if tau_ext.size != self.dof_ or q.size != self.dof_:
-            raise BufferError(
-                f"Expected joint position and torque with {self.dof_} dof, got {q.size()} amd {tau_ext.size()} dof."
-            )
-
-        jacobian_inv = np.linalg.pinv(jacobian, rcond=0.05)
-
-        f_ext = jacobian_inv.T @ tau_ext
-        f_ext = np.where(
-            abs(f_ext) > self.f_threshold_,
-            self.dx_gain_ @ np.sign(f_ext) * (abs(f_ext) - self.f_threshold_),
-            0.0,
-        )
-
-        dq = self.dq_gain_ @ jacobian_inv @ f_ext
-        self.dq_ = (1.0 - self.smooth_) * self.dq_ + self.smooth_ * dq
-        return self.dq_, f_ext
+from .admittance_controller import AdmittanceController
 
 
 class AdmittanceControlNode(Node):
     def __init__(self, node_name="admittance_control_node") -> None:
         super().__init__(node_name=node_name)
 
         # parameters
-        self.declare_parameter("model", "med7")
-        self.declare_parameter("end_link_name", "lbr_link_ee")
-        self.declare_parameter("root_link_name", "lbr_link_0")
-        self.declare_parameter("command_rate", 100.0)
-        self.declare_parameter("buffer_len", 20)
+        self.declare_parameter("robot_description", "")
+        self.declare_parameter("base_link", "lbr_link_0")
+        self.declare_parameter("end_effector_link", "lbr_link_ee")
 
-        self.model_ = str(self.get_parameter("model").value)
-        self.end_link_name_ = str(self.get_parameter("end_link_name").value)
-        self.root_link_name_ = str(self.get_parameter("root_link_name").value)
-        self.dt_ = 1.0 / float(self.get_parameter("command_rate").value)
+        self.init_ = False
+        self.lbr_state_ = LBRState()
 
-        # controller
-        path = os.path.join(
-            get_package_share_directory("lbr_description"),
-            "urdf",
-            self.model_,
-            f"{self.model_}.urdf.xacro",
+        self.controller_ = AdmittanceController(
+            robot_description=str(self.get_parameter("robot_description").value),
+            base_link=str(self.get_parameter("base_link").value),
+            end_effector_link=str(self.get_parameter("end_effector_link").value),
         )
-        self.urdf_string_ = xacro.process(path)
-
-        self.controller_ = Controller(
-            urdf_string=self.urdf_string_,
-            end_link_name=self.end_link_name_,
-            root_link_name=self.root_link_name_,
-        )
-
-        self.lbr_state_ = None
 
         # publishers and subscribers
         self.lbr_state_sub_ = self.create_subscription(
-            LBRState, "/lbr_state", self.lbr_state_cb_, qos_profile_sensor_data
+            LBRState,
+            "/lbr_state",
+            self.on_lbr_state_,
+            QoSProfile(
+                depth=1,
+                reliability=ReliabilityPolicy.RELIABLE,
+                deadline=Duration(nanoseconds=10 * 1e6),  # 10 milliseconds
+            ),
         )
         self.lbr_command_pub_ = self.create_publisher(
-            LBRCommand, "/lbr_command", qos_profile_sensor_data
+            LBRCommand,
+            "/lbr_command",
+            QoSProfile(
+                depth=1,
+                reliability=ReliabilityPolicy.RELIABLE,
+                deadline=Duration(nanoseconds=10 * 1e6),  # 10 milliseconds
+            ),
         )
-        self.lbr_command_timer_ = self.create_timer(self.dt_, self.timer_cb_)
 
-        self.joint_position_buffer_len_ = int(self.get_parameter("buffer_len").value)
-        self.joint_position_buffer_ = []
+    def on_lbr_state_(self, lbr_state: LBRState) -> None:
+        self.smooth_lbr_state_(lbr_state, 0.95)
 
-        self.external_torque_buffer_len_ = int(self.get_parameter("buffer_len").value)
-        self.external_torque_buffer_ = []
+        lbr_command = self.controller_(self.lbr_state_)
+        self.lbr_command_pub_.publish(lbr_command)
 
-    def lbr_state_cb_(self, msg: LBRState) -> None:
-        self.lbr_state_ = msg
-
-    def timer_cb_(self) -> None:
-        if not self.lbr_state_:
+    def smooth_lbr_state_(self, lbr_state: LBRState, alpha: float):
+        if not self.init_:
+            self.lbr_state_ = lbr_state
+            self.init_ = True
             return
-        # compute control
-        q = np.array(self.lbr_state_.measured_joint_position.tolist())
-
-        if len(self.joint_position_buffer_) > self.joint_position_buffer_len_:
-            self.joint_position_buffer_.pop(0)
-        self.joint_position_buffer_.append(q)
-
-        q = np.zeros_like(q)
-        for qi in self.joint_position_buffer_:
-            q += qi / len(self.joint_position_buffer_)
-
-        tau_ext = np.array(self.lbr_state_.external_torque.tolist())
-
-        if len(self.external_torque_buffer_) > self.external_torque_buffer_len_:
-            self.external_torque_buffer_.pop(0)
-        self.external_torque_buffer_.append(tau_ext)
-
-        tau_ext = np.zeros_like(tau_ext)
-        for tau_ext_i in self.external_torque_buffer_:
-            tau_ext += tau_ext_i / len(self.external_torque_buffer_)
 
-        dq, f_ext = self.controller_(q, tau_ext)
+        self.lbr_state_.measured_joint_position = (
+            (1 - alpha) * np.array(self.lbr_state_.measured_joint_position.tolist())
+            + alpha * np.array(lbr_state.measured_joint_position.tolist())
+        ).data
 
-        # command
-        command = LBRCommand()
-        command.joint_position = (q + dq * self.dt_).data
-        self.lbr_command_pub_.publish(command)
+        self.lbr_state_.external_torque = (
+            (1 - alpha) * np.array(self.lbr_state_.external_torque.tolist())
+            + alpha * np.array(lbr_state.external_torque.tolist())
+        ).data
 
 
 def main(args=None):

lbr_demos/lbr_fri_ros2_advanced_python_demos/lbr_fri_ros2_advanced_python_demos/admittance_controller.py

@@ -0,0 +1,60 @@
+import numpy as np
+import optas
+
+from lbr_fri_msgs.msg import LBRCommand, LBRState
+
+
+class AdmittanceController(object):
+    def __init__(
+        self,
+        robot_description: str,
+        base_link: str = "lbr_link_0",
+        end_effector_link: str = "lbr_link_ee",
+        f_ext_th: np.ndarray = np.array([2.0, 2.0, 2.0, 0.5, 0.5, 0.5]),
+        dq_gain: np.ndarray = np.array([1.5, 1.5, 1.5, 1.5, 1.5, 1.5, 1.5]),
+        dx_gain: np.ndarray = np.array([1.0, 1.0, 1.0, 20.0, 40.0, 60.0]),
+    ) -> None:
+        self.lbr_command_ = LBRCommand()
+
+        robot = optas.RobotModel(urdf_string=robot_description)
+        J = robot.get_geometric_jacobian_function(end_effector_link, base_link)
+        self.jacobian_ = lambda q: J(q, numpy_output=True)
+
+        self.dof_ = robot.ndof
+        self.jacobian_inv_ = np.zeros((self.dof_, 6))
+        self.q = np.zeros(self.dof_)
+        self.dq_ = np.zeros(self.dof_)
+        self.tau_ext_ = np.zeros(6)
+        self.dq_gain_ = np.diag(dq_gain)
+        self.dx_gain_ = np.diag(dx_gain)
+        self.f_ext_ = np.zeros(6)
+        self.f_ext_th_ = f_ext_th
+        self.alpha_ = 0.99
+
+    def __call__(self, lbr_state: LBRState) -> LBRCommand:
+        self.q_ = np.array(lbr_state.measured_joint_position.tolist())
+        self.tau_ext_ = np.array(lbr_state.external_torque.tolist())
+
+        self.jacobian_ = self.jacobian_(self.q_)
+
+        self.jacobian_inv_ = np.linalg.pinv(self.jacobian_, rcond=0.05)
+        self.f_ext_ = self.jacobian_inv_.T @ self.tau_ext_
+
+        self.f_ext_ = np.where(
+            abs(self.f_ext_) > self.f_ext_th_,
+            self.dx_gain_ @ np.sign(self.f_ext_) * (abs(self.f_ext_) - self.f_ext_th_),
+            0.0,
+        )
+
+        # additional smoothing required in python
+        self.dq_ = (
+            self.alpha_ * self.dq_
+            + (1 - self.alpha_) * self.dq_gain_ @ self.jacobian_inv_ @ self.f_ext_
+        )
+
+        self.lbr_command_.joint_position = (
+            np.array(lbr_state.measured_joint_position.tolist())
+            + lbr_state.sample_time * self.dq_
+        ).data
+
+        return self.lbr_command_

lbr_demos/lbr_fri_ros2_advanced_python_demos/package.xml

@@ -2,12 +2,13 @@
 <?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="3">
   <name>lbr_fri_ros2_advanced_python_demos</name>
-  <version>1.0.0</version>
+  <version>1.1.0</version>
   <description>Advanced Python demos for the lbr_fri_ros2.</description>
   <maintainer email="[email protected]">mhubii</maintainer>
   <license>MIT</license>
 
   <exec_depend>lbr_fri_msgs</exec_depend>
+  <exec_depend>lbr_fri_ros2</exec_depend>
   <exec_depend>rclpy</exec_depend>
 
   <test_depend>ament_copyright</test_depend>

lbr_demos/lbr_fri_ros2_advanced_python_demos/setup.py

@@ -1,14 +1,17 @@
+import glob
+
 from setuptools import setup
 
 package_name = "lbr_fri_ros2_advanced_python_demos"
 
 setup(
     name=package_name,
-    version="0.0.0",
+    version="1.1.0",
     packages=[package_name],
     data_files=[
         ("share/ament_index/resource_index/packages", ["resource/" + package_name]),
         ("share/" + package_name, ["package.xml"]),
+        ("share/" + package_name + "/launch", glob.glob("launch/*.py")),
     ],
     install_requires=["setuptools"],
     zip_safe=True,

lbr_demos/lbr_fri_ros2_cpp_demos/package.xml

@@ -2,7 +2,7 @@
 <?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="3">
   <name>lbr_fri_ros2_cpp_demos</name>
-  <version>1.0.0</version>
+  <version>1.1.0</version>
   <description>C++ demos for the lbr_fri_ros2.</description>
   <maintainer email="[email protected]">mhubii</maintainer>
   <license>MIT</license>