From 817dfeaa0f63883ab6595c2ec4c4e693151eba83 Mon Sep 17 00:00:00 2001 From: Bill Finger Date: Wed, 2 Aug 2023 16:57:01 +0300 Subject: [PATCH] =?UTF-8?q?=F0=9F=9A=A7=20add=20impedance=20controller=20n?= =?UTF-8?q?ode?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- rbs_bringup/launch/bringup.launch.py | 4 +- .../CMakeLists.txt | 35 +++++++++++++++++ .../cartesian_impedance_controller.hpp | 39 +++++++++++++++++++ .../package.xml | 18 +++++++++ .../src/cartesian_impedance_controller.cpp | 14 +++++++ rbs_simulation/launch/simulation.launch.py | 6 +-- rbs_simulation/mujoco_model/current_mj.xml | 5 +++ 7 files changed, 116 insertions(+), 5 deletions(-) create mode 100644 rbs_controllers/cartesian_impedance_controller/CMakeLists.txt create mode 100644 rbs_controllers/cartesian_impedance_controller/include/cartesian_impedance_controller/cartesian_impedance_controller.hpp create mode 100644 rbs_controllers/cartesian_impedance_controller/package.xml create mode 100644 rbs_controllers/cartesian_impedance_controller/src/cartesian_impedance_controller.cpp diff --git a/rbs_bringup/launch/bringup.launch.py b/rbs_bringup/launch/bringup.launch.py index 32caa7f..0545b75 100644 --- a/rbs_bringup/launch/bringup.launch.py +++ b/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?") diff --git a/rbs_controllers/cartesian_impedance_controller/CMakeLists.txt b/rbs_controllers/cartesian_impedance_controller/CMakeLists.txt new file mode 100644 index 0000000..c3b26f4 --- /dev/null +++ b/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( REQUIRED) + +# add_executable(cartesian_impedance_controller src/cartesian_impedance_controller.cpp) +# target_include_directories(cartesian_impedance_controller PUBLIC +# $ +# $) +# 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() diff --git a/rbs_controllers/cartesian_impedance_controller/include/cartesian_impedance_controller/cartesian_impedance_controller.hpp b/rbs_controllers/cartesian_impedance_controller/include/cartesian_impedance_controller/cartesian_impedance_controller.hpp new file mode 100644 index 0000000..0560abb --- /dev/null +++ b/rbs_controllers/cartesian_impedance_controller/include/cartesian_impedance_controller/cartesian_impedance_controller.hpp @@ -0,0 +1,39 @@ +#include +#include +#include +#include + +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; + + +}; + +} \ No newline at end of file diff --git a/rbs_controllers/cartesian_impedance_controller/package.xml b/rbs_controllers/cartesian_impedance_controller/package.xml new file mode 100644 index 0000000..5a81605 --- /dev/null +++ b/rbs_controllers/cartesian_impedance_controller/package.xml @@ -0,0 +1,18 @@ + + + + cartesian_impedance_controller + 0.0.0 + TODO: Package description + bill-finger + TODO: License declaration + + ament_cmake + + ament_lint_auto + ament_lint_common + + + ament_cmake + + diff --git a/rbs_controllers/cartesian_impedance_controller/src/cartesian_impedance_controller.cpp b/rbs_controllers/cartesian_impedance_controller/src/cartesian_impedance_controller.cpp new file mode 100644 index 0000000..aa9a431 --- /dev/null +++ b/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 + +namespace cartesian_impedance_controller +{ +CartesianImpedanceController::CartesianImpedanceController() +: Base::CartesianControllerBase(), +MotionBase::CartesianMotionController(), +ForceBase::CartesianForceController() +{ +} + +} \ No newline at end of file diff --git a/rbs_simulation/launch/simulation.launch.py b/rbs_simulation/launch/simulation.launch.py index 37423fb..1470229 100644 --- a/rbs_simulation/launch/simulation.launch.py +++ b/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', diff --git a/rbs_simulation/mujoco_model/current_mj.xml b/rbs_simulation/mujoco_model/current_mj.xml index 465d8f3..d978eae 100644 --- a/rbs_simulation/mujoco_model/current_mj.xml +++ b/rbs_simulation/mujoco_model/current_mj.xml @@ -2,6 +2,9 @@ + + + @@ -11,6 +14,8 @@ + +