🚧 add impedance controller node

rbs_bringup/launch/bringup.launch.py

@@ -83,7 +83,7 @@ def generate_launch_description():
     declared_arguments.append(
         DeclareLaunchArgument(
             "start_joint_controller",
-            default_value="true",
+            default_value="false",
             description="Enable headless mode for robot control",
         )
     )
@@ -126,7 +126,7 @@ def generate_launch_description():
         )
     )
     declared_arguments.append(
-        DeclareLaunchArgument("with_gripper", default_value="true", description="With gripper or not?")
+        DeclareLaunchArgument("with_gripper", default_value="false", description="With gripper or not?")
     )
     declared_arguments.append(
         DeclareLaunchArgument("launch_rviz", default_value="true", description="Launch RViz?")

rbs_controllers/cartesian_impedance_controller/CMakeLists.txt

@@ -0,0 +1,35 @@
+cmake_minimum_required(VERSION 3.8)
+project(cartesian_impedance_controller)
+
+if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
+  add_compile_options(-Wall -Wextra -Wpedantic)
+endif()
+
+# find dependencies
+find_package(ament_cmake REQUIRED)
+# uncomment the following section in order to fill in
+# further dependencies manually.
+# find_package(<dependency> REQUIRED)
+
+# add_executable(cartesian_impedance_controller src/cartesian_impedance_controller.cpp)
+# target_include_directories(cartesian_impedance_controller PUBLIC
+#   $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
+#   $<INSTALL_INTERFACE:include>)
+# target_compile_features(cartesian_impedance_controller PUBLIC c_std_99 cxx_std_17)  # Require C99 and C++17
+
+# install(TARGETS cartesian_impedance_controller
+#   DESTINATION lib/${PROJECT_NAME})
+
+if(BUILD_TESTING)
+  find_package(ament_lint_auto REQUIRED)
+  # the following line skips the linter which checks for copyrights
+  # comment the line when a copyright and license is added to all source files
+  set(ament_cmake_copyright_FOUND TRUE)
+  # the following line skips cpplint (only works in a git repo)
+  # comment the line when this package is in a git repo and when
+  # a copyright and license is added to all source files
+  set(ament_cmake_cpplint_FOUND TRUE)
+  ament_lint_auto_find_test_dependencies()
+endif()
+
+ament_package()

rbs_controllers/cartesian_impedance_controller/include/cartesian_impedance_controller/cartesian_impedance_controller.hpp

@@ -0,0 +1,39 @@
+#include <cartesian_controller_base/cartesian_controller_base.h>
+#include <cartesian_force_controller/cartesian_force_controller.h>
+#include <cartesian_motion_controller/cartesian_motion_controller.h>
+#include <controller_interface/controller_interface.hpp>
+
+namespace cartesian_impedance_controller
+{
+
+class CartesianImpedanceController : 
+public cartesian_motion_controller::CartesianMotionController,
+public cartesian_force_controller::CartesianForceController
+{
+    public:
+    CartesianImpedanceController();
+
+    virtual LifecycleNodeInterface::CallbackReturn on_init() override;
+
+    rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn on_configure(
+        const rclcpp_lifecycle::State & previous_state) override;
+    rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn on_activate(
+        const rclcpp_lifecycle::State & previous_state) override;
+    rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn on_deactivate(
+        const rclcpp_lifecycle::State & previous_state) override;
+    controller_interface::return_type update(const rclcpp::Time & time, const rclcpp::Duration & period) override;
+
+    using Base = cartesian_controller_base::CartesianControllerBase;
+    using MotionBase = cartesian_motion_controller::CartesianMotionController;
+    using ForceBase = cartesian_force_controller::CartesianForceController;
+    
+    private:
+    ctrl::Vector6D computeImpedance();
+    ctrl::Matrix6D m_stiffness;
+    ctrl::Matrix6D m_damping;
+    std::string m_compliance_ref_link;
+    
+
+};
+
+}

rbs_controllers/cartesian_impedance_controller/package.xml

@@ -0,0 +1,18 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+  <name>cartesian_impedance_controller</name>
+  <version>0.0.0</version>
+  <description>TODO: Package description</description>
+  <maintainer email="ur.narmak@gmail.com">bill-finger</maintainer>
+  <license>TODO: License declaration</license>
+
+  <buildtool_depend>ament_cmake</buildtool_depend>
+
+  <test_depend>ament_lint_auto</test_depend>
+  <test_depend>ament_lint_common</test_depend>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>

rbs_controllers/cartesian_impedance_controller/src/cartesian_impedance_controller.cpp

@@ -0,0 +1,14 @@
+#include "cartesian_controller_base/Utility.h"
+#include "controller_interface/controller_interface.hpp"
+#include <cartesian_impedance_controller/cartesian_impedance_controller.hpp>
+
+namespace cartesian_impedance_controller
+{
+CartesianImpedanceController::CartesianImpedanceController()
+: Base::CartesianControllerBase(),
+MotionBase::CartesianMotionController(),
+ForceBase::CartesianForceController()
+{
+}
+
+}

rbs_simulation/launch/simulation.launch.py

@@ -43,18 +43,18 @@ def generate_launch_description():
             PythonLaunchDescriptionSource(
                 [os.path.join(get_package_share_directory('ros_gz_sim'),
                             'launch', 'gz_sim.launch.py')]),
-            launch_arguments=[('ign_args', [' -r ',world_config_file, " -s"])],
+            launch_arguments=[('gz_args', [' -r ',world_config_file, " -s"])],
             condition=UnlessCondition(gazebo_gui))
     
     gazebo = IncludeLaunchDescription(
             PythonLaunchDescriptionSource(
                 [os.path.join(get_package_share_directory('ros_gz_sim'),
                             'launch', 'gz_sim.launch.py')]),
-            launch_arguments=[('ign_args', [' -r ',world_config_file])],
+            launch_arguments=[('gz_args', [' -r ',world_config_file])],
             condition=IfCondition(gazebo_gui))
     
     # Spawn robot
-    gazebo_spawn_robot = Node(package='ros_ign_gazebo', executable='create',
+    gazebo_spawn_robot = Node(package='ros_gz_sim', executable='create',
                  arguments=[
                     '-name', rbs_robot_type,
                     '-x', '0.0',

rbs_simulation/mujoco_model/current_mj.xml

@@ -2,6 +2,9 @@
   <compiler angle="radian" meshdir="meshes/"/>
   <default/>
   <asset>
+    <texture type="skybox" builtin="gradient" rgb1="0.9 0.9 0.9" rgb2="0.5 0.5 0.5" width="512" height="512"/>
+    <texture name="texplane" type="2d" builtin="checker" rgb1=".25 .25 .25" rgb2=".3 .3 .3" width="512" height="512" mark="cross" markrgb=".8 .8 .8"/>
+    <material name="matplane" reflectance="0.2" texture="texplane" texrepeat="1 1" texuniform="true"/>
     <mesh name="base" file="base.stl"/>
     <mesh name="shoulder" file="shoulder.stl"/>
     <mesh name="upperarm" file="upperarm.stl"/>
@@ -11,6 +14,8 @@
     <mesh name="wrist3" file="wrist3.stl"/>
   </asset>
   <worldbody>
+    <light directional="true" diffuse=".8 .8 .8" specular=".2 .2 .2" pos="0 0 5" dir="0 0 -1" castshadow="false"/>
+    <geom name="floor" pos="0 0 -0.0" size="0 0 1" type="plane" material="matplane"/>
     <geom quat="-1 0 0 0" type="mesh" contype="0" conaffinity="0" group="1" density="0" mesh="base"/>
     <geom quat="-1 0 0 0" type="mesh" mesh="base"/>
     <body name="shoulder_link" pos="0 0 0.1625" quat="0 0 0 1" >