From 522d30bfde5bdf0924251bdd29c173eb3d03f2aa Mon Sep 17 00:00:00 2001 From: munseng Date: Wed, 22 Feb 2023 19:12:45 +0800 Subject: [PATCH 1/4] Init joint impedance controller --- flexiv_controllers/CMakeLists.txt | 68 +++++++++++++++ .../config/flexiv_controllers.yaml | 26 ++++++ .../flexiv_controllers_plugin.xml | 7 ++ .../joint_impedance_controller.h | 56 ++++++++++++ flexiv_controllers/package.xml | 31 +++++++ .../src/joint_impedance_controller.cpp | 87 +++++++++++++++++++ 6 files changed, 275 insertions(+) create mode 100644 flexiv_controllers/CMakeLists.txt create mode 100644 flexiv_controllers/config/flexiv_controllers.yaml create mode 100644 flexiv_controllers/flexiv_controllers_plugin.xml create mode 100644 flexiv_controllers/include/flexiv_controllers/joint_impedance_controller.h create mode 100644 flexiv_controllers/package.xml create mode 100644 flexiv_controllers/src/joint_impedance_controller.cpp diff --git a/flexiv_controllers/CMakeLists.txt b/flexiv_controllers/CMakeLists.txt new file mode 100644 index 0000000..547d2b0 --- /dev/null +++ b/flexiv_controllers/CMakeLists.txt @@ -0,0 +1,68 @@ +cmake_minimum_required(VERSION 3.5) +project(flexiv_controllers) + +set(CMAKE_CXX_STANDARD 14) +set(CMAKE_CXX_STANDARD_REQUIRED ON) + +find_package(catkin REQUIRED COMPONENTS + controller_interface + flexiv_msgs + hardware_interface + pluginlib + realtime_tools + roscpp + urdf +) + +catkin_package( + INCLUDE_DIRS + include + CATKIN_DEPENDS + controller_interface + flexiv_msgs + hardware_interface + pluginlib + realtime_tools + roscpp + urdf + LIBRARIES + ${PROJECT_NAME} +) + +# Build +add_library(${PROJECT_NAME} + src/joint_impedance_controller.cpp +) +target_include_directories( + ${PROJECT_NAME} + PRIVATE + ${CMAKE_CURRENT_SOURCE_DIR}/include + ${catkin_INCLUDE_DIRS} +) + +target_link_libraries(${PROJECT_NAME} PRIVATE ${catkin_LIBRARIES}) + +# INSTALL +# Install headers +install(DIRECTORY include/${PROJECT_NAME}/ + DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} +) + +# Install targets +install(TARGETS ${PROJECT_NAME} + ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION} +) + +# Install plugins +install(FILES flexiv_controllers_plugin.xml + DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} +) + +install(DIRECTORY launch + DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} +) +install(DIRECTORY config + DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} +) diff --git a/flexiv_controllers/config/flexiv_controllers.yaml b/flexiv_controllers/config/flexiv_controllers.yaml new file mode 100644 index 0000000..69edc8a --- /dev/null +++ b/flexiv_controllers/config/flexiv_controllers.yaml @@ -0,0 +1,26 @@ +joint_impedance_controller: + type: flexiv_controllers/JointImpedanceController + joint_names: + - joint1 + - joint2 + - joint3 + - joint4 + - joint5 + - joint6 + - joint7 + k_p: + - 3000.0 + - 3000.0 + - 800.0 + - 800.0 + - 200.0 + - 200.0 + - 200.0 + k_d: + - 80.0 + - 80.0 + - 40.0 + - 40.0 + - 8.0 + - 8.0 + - 8.0 diff --git a/flexiv_controllers/flexiv_controllers_plugin.xml b/flexiv_controllers/flexiv_controllers_plugin.xml new file mode 100644 index 0000000..850a6fc --- /dev/null +++ b/flexiv_controllers/flexiv_controllers_plugin.xml @@ -0,0 +1,7 @@ + + + The joint impedance controller commands a group of joints in effort interface. + + diff --git a/flexiv_controllers/include/flexiv_controllers/joint_impedance_controller.h b/flexiv_controllers/include/flexiv_controllers/joint_impedance_controller.h new file mode 100644 index 0000000..f0a334b --- /dev/null +++ b/flexiv_controllers/include/flexiv_controllers/joint_impedance_controller.h @@ -0,0 +1,56 @@ +/** + * @file joint_impedance_controller.hpp + * Joint impedance control as ROS controller. Adapted from + * ros_controllers/effort_controllers + * @copyright Copyright (C) 2016-2021 Flexiv Ltd. All Rights Reserved. + * @author Flexiv + */ + +#ifndef FLEXIV_CONTROLLERS__JOINT_IMPEDANCE_CONTROLLER_HPP_ +#define FLEXIV_CONTROLLERS__JOINT_IMPEDANCE_CONTROLLER_HPP_ + +#include +#include +#include + +// ROS +#include +#include +#include +#include +#include +#include + +// Flexiv + +namespace flexiv_controllers { + +class JointImpedanceController : public controller_interface::Controller< + hardware_interface::EffortJointInterface> +{ +public: + // JointImpedanceController(); + // ~JointImpedanceController(); + + bool init( + hardware_interface::EffortJointInterface* hw, ros::NodeHandle& nh); + + void update(const ros::Time& time, const ros::Duration& period); + + void starting(const ros::Time& time); + + void setCommand(double pos_command); + + void setCommand(double pos_command, double vel_command); + +private: + std::vector joints_; + std::vector k_p_; + std::vector k_d_; + std::array q_init_; + static const size_t num_joints_ = 7; +}; + +} // namespace flexiv_controllers + +#endif // FLEXIV_CONTROLLERS__JOINT_IMPEDANCE_CONTROLLER_HPP_ diff --git a/flexiv_controllers/package.xml b/flexiv_controllers/package.xml new file mode 100644 index 0000000..4503d40 --- /dev/null +++ b/flexiv_controllers/package.xml @@ -0,0 +1,31 @@ + + + flexiv_controllers + 0.7.0 + Flexiv custom ROS controllers + Mun Seng Phoon + Apache License 2.0 + Mun Seng Phoon + + catkin + + controller_interface + hardware_interface + pluginlib + realtime_tools + roscpp + urdf + flexiv_msgs + + controller_interface + hardware_interface + roscpp + pluginlib + realtime_tools + urdf + flexiv_msgs + + + + + diff --git a/flexiv_controllers/src/joint_impedance_controller.cpp b/flexiv_controllers/src/joint_impedance_controller.cpp new file mode 100644 index 0000000..1a3edd0 --- /dev/null +++ b/flexiv_controllers/src/joint_impedance_controller.cpp @@ -0,0 +1,87 @@ +#include +#include + +namespace flexiv_controllers { + +bool JointImpedanceController::init( + hardware_interface::EffortJointInterface* hw, ros::NodeHandle& nh) +{ + // Get joint name from parameter server + std::vector joint_names; + if (!nh.getParam("joint_names", joint_names)) { + ROS_ERROR("JointImpedanceController: Could not parse joint names"); + } + if (joint_names.size() != 7) { + ROS_ERROR_STREAM( + "JointImpedanceController: Wrong number of joint names, got " + << joint_names.size() << " instead of 7 names!"); + return false; + } + + // Get controller gains + if (!nh.getParam("k_p", k_p_)) { + ROS_ERROR("JointImpedanceController: No k_p parameters provided!"); + return false; + } + if (!nh.getParam("k_d", k_d_)) { + ROS_ERROR("JointImpedanceController: No k_d parameters provided!"); + return false; + } + if (k_p_.size() != num_joints_) { + ROS_ERROR("JointImpedanceController: k_p should be of size 7"); + return false; + } + if (k_d_.size() != num_joints_) { + ROS_ERROR("JointImpedanceController: k_d should be of size 7"); + return false; + } + for (auto i = 0ul; i < k_p_.size(); i++) { + if (k_p_[i] < 0 || k_d_[i] < 0) { + ROS_ERROR("JointImpedanceController: Wrong impedance parameters!"); + return false; + } + } + + // Get joint handles from hardware interface + joints_.resize(num_joints_); + for (std::size_t i = 0; i < num_joints_; ++i) { + try { + joints_[i] = hw->getHandle(joint_names[i]); + } catch (const hardware_interface::HardwareInterfaceException& e) { + ROS_ERROR_STREAM( + "JointImpedanceController: Exception getting joint handles: " + << e.what()); + return false; + } + } + + return true; +} + +void JointImpedanceController::starting(const ros::Time& /*time*/) +{ + for (std::size_t i = 0; i < num_joints_; ++i) { + q_init_[i] = joints_[i].getPosition(); + } +} + +void JointImpedanceController::update( + const ros::Time& /*time*/, const ros::Duration& /*period*/) +{ + std::array q_des = q_init_; + + for (std::size_t i = 0; i < num_joints_; ++i) { + // Get current joint positions and velocities + double q = joints_[i].getPosition(); + double dq = joints_[i].getVelocity(); + + // Compute torque + double tau = k_p_[i] * (q_des[i] - q) - k_d_[i] * (dq); + joints_[i].setCommand(tau); + } +} + +} // namespace flexiv_controllers + +PLUGINLIB_EXPORT_CLASS(flexiv_controllers::JointImpedanceController, + controller_interface::ControllerBase) From ee7f8f168e974e9dd266bd6f06b3ed5a016e14bb Mon Sep 17 00:00:00 2001 From: munseng Date: Thu, 23 Feb 2023 15:51:16 +0800 Subject: [PATCH 2/4] Update Flexiv controllers config and CMakeLists --- flexiv_bringup/config/rizon_controllers.yaml | 20 ++++++++++++++ flexiv_bringup/package.xml | 1 + flexiv_controllers/CMakeLists.txt | 23 +++++++--------- .../config/flexiv_controllers.yaml | 26 ------------------- .../flexiv_controllers_plugin.xml | 10 +++---- flexiv_controllers/package.xml | 4 +++ .../src/joint_impedance_controller.cpp | 5 ++-- 7 files changed, 42 insertions(+), 47 deletions(-) delete mode 100644 flexiv_controllers/config/flexiv_controllers.yaml diff --git a/flexiv_bringup/config/rizon_controllers.yaml b/flexiv_bringup/config/rizon_controllers.yaml index a537d02..1d83617 100644 --- a/flexiv_bringup/config/rizon_controllers.yaml +++ b/flexiv_bringup/config/rizon_controllers.yaml @@ -33,3 +33,23 @@ position_joint_trajectory_controller: stop_trajectory_duration: 0.5 state_publish_rate: 100.0 action_monitor_rate: 20.0 + +joint_impedance_controller: + type: flexiv_controllers/JointImpedanceController + joints: *robot_joints + k_p: + - 3000.0 + - 3000.0 + - 800.0 + - 800.0 + - 200.0 + - 200.0 + - 200.0 + k_d: + - 80.0 + - 80.0 + - 40.0 + - 40.0 + - 8.0 + - 8.0 + - 8.0 diff --git a/flexiv_bringup/package.xml b/flexiv_bringup/package.xml index 3773f4c..775f500 100644 --- a/flexiv_bringup/package.xml +++ b/flexiv_bringup/package.xml @@ -11,6 +11,7 @@ flexiv_description flexiv_hardware + flexiv_controllers xacro diff --git a/flexiv_controllers/CMakeLists.txt b/flexiv_controllers/CMakeLists.txt index 547d2b0..0fdf07f 100644 --- a/flexiv_controllers/CMakeLists.txt +++ b/flexiv_controllers/CMakeLists.txt @@ -6,8 +6,10 @@ set(CMAKE_CXX_STANDARD_REQUIRED ON) find_package(catkin REQUIRED COMPONENTS controller_interface + controller_manager flexiv_msgs hardware_interface + joint_limits_interface pluginlib realtime_tools roscpp @@ -19,8 +21,10 @@ catkin_package( include CATKIN_DEPENDS controller_interface + controller_manager flexiv_msgs hardware_interface + joint_limits_interface pluginlib realtime_tools roscpp @@ -43,26 +47,19 @@ target_include_directories( target_link_libraries(${PROJECT_NAME} PRIVATE ${catkin_LIBRARIES}) # INSTALL -# Install headers -install(DIRECTORY include/${PROJECT_NAME}/ - DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} -) - # Install targets install(TARGETS ${PROJECT_NAME} ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION} + RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} ) -# Install plugins -install(FILES flexiv_controllers_plugin.xml - DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} +# Install headers +install(DIRECTORY include/${PROJECT_NAME}/ + DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} ) -install(DIRECTORY launch - DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} -) -install(DIRECTORY config +# Install plugins +install(FILES flexiv_controllers_plugin.xml DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} ) diff --git a/flexiv_controllers/config/flexiv_controllers.yaml b/flexiv_controllers/config/flexiv_controllers.yaml deleted file mode 100644 index 69edc8a..0000000 --- a/flexiv_controllers/config/flexiv_controllers.yaml +++ /dev/null @@ -1,26 +0,0 @@ -joint_impedance_controller: - type: flexiv_controllers/JointImpedanceController - joint_names: - - joint1 - - joint2 - - joint3 - - joint4 - - joint5 - - joint6 - - joint7 - k_p: - - 3000.0 - - 3000.0 - - 800.0 - - 800.0 - - 200.0 - - 200.0 - - 200.0 - k_d: - - 80.0 - - 80.0 - - 40.0 - - 40.0 - - 8.0 - - 8.0 - - 8.0 diff --git a/flexiv_controllers/flexiv_controllers_plugin.xml b/flexiv_controllers/flexiv_controllers_plugin.xml index 850a6fc..bc96b21 100644 --- a/flexiv_controllers/flexiv_controllers_plugin.xml +++ b/flexiv_controllers/flexiv_controllers_plugin.xml @@ -1,7 +1,7 @@ - - The joint impedance controller commands a group of joints in effort interface. + + + The joint impedance controller commands a group of joints in effort interface. + - + \ No newline at end of file diff --git a/flexiv_controllers/package.xml b/flexiv_controllers/package.xml index 4503d40..2de1775 100644 --- a/flexiv_controllers/package.xml +++ b/flexiv_controllers/package.xml @@ -10,7 +10,9 @@ catkin controller_interface + controller_manager hardware_interface + joint_limits_interface pluginlib realtime_tools roscpp @@ -18,7 +20,9 @@ flexiv_msgs controller_interface + controller_manager hardware_interface + joint_limits_interface roscpp pluginlib realtime_tools diff --git a/flexiv_controllers/src/joint_impedance_controller.cpp b/flexiv_controllers/src/joint_impedance_controller.cpp index 1a3edd0..3c12328 100644 --- a/flexiv_controllers/src/joint_impedance_controller.cpp +++ b/flexiv_controllers/src/joint_impedance_controller.cpp @@ -8,7 +8,7 @@ bool JointImpedanceController::init( { // Get joint name from parameter server std::vector joint_names; - if (!nh.getParam("joint_names", joint_names)) { + if (!nh.getParam("joints", joint_names)) { ROS_ERROR("JointImpedanceController: Could not parse joint names"); } if (joint_names.size() != 7) { @@ -83,5 +83,4 @@ void JointImpedanceController::update( } // namespace flexiv_controllers -PLUGINLIB_EXPORT_CLASS(flexiv_controllers::JointImpedanceController, - controller_interface::ControllerBase) +PLUGINLIB_EXPORT_CLASS(flexiv_controllers::JointImpedanceController, controller_interface::ControllerBase) From 9fdcf69241269273f173f95b0ac365c250de008e Mon Sep 17 00:00:00 2001 From: munseng Date: Thu, 23 Feb 2023 16:03:21 +0800 Subject: [PATCH 3/4] Set current position to init after enable robot --- flexiv_hardware/src/flexiv_hardware_interface.cpp | 9 ++++----- 1 file changed, 4 insertions(+), 5 deletions(-) diff --git a/flexiv_hardware/src/flexiv_hardware_interface.cpp b/flexiv_hardware/src/flexiv_hardware_interface.cpp index e68e18a..61ea50b 100644 --- a/flexiv_hardware/src/flexiv_hardware_interface.cpp +++ b/flexiv_hardware/src/flexiv_hardware_interface.cpp @@ -28,8 +28,7 @@ FlexivHardwareInterface::FlexivHardwareInterface() , velocity_controller_running_(false) , effort_controller_running_(false) , controllers_initialized_(false) -{ -} +{ } bool FlexivHardwareInterface::init( ros::NodeHandle& root_nh, ros::NodeHandle& robot_hw_nh) @@ -157,6 +156,9 @@ bool FlexivHardwareInterface::initRobot() } ROS_INFO("Robot is now operational."); + // get current position and set to initial position + setInitPosition(); + return true; } @@ -335,9 +337,6 @@ void FlexivHardwareInterface::doSwitch( velocity_controller_running_ = false; effort_controller_running_ = false; - // get current position and set to initial position - setInitPosition(); - // Set to joint position mode robot_->setMode(flexiv::MODE_JOINT_POSITION); From db0daa5b6731723f01fc0d70adaba1a082a7ed41 Mon Sep 17 00:00:00 2001 From: munseng Date: Thu, 23 Feb 2023 18:18:40 +0800 Subject: [PATCH 4/4] Add joint impedance controller command subscriber --- .../joint_impedance_controller.h | 30 ++++- .../src/joint_impedance_controller.cpp | 113 +++++++++++++++++- 2 files changed, 134 insertions(+), 9 deletions(-) diff --git a/flexiv_controllers/include/flexiv_controllers/joint_impedance_controller.h b/flexiv_controllers/include/flexiv_controllers/joint_impedance_controller.h index f0a334b..41daa73 100644 --- a/flexiv_controllers/include/flexiv_controllers/joint_impedance_controller.h +++ b/flexiv_controllers/include/flexiv_controllers/joint_impedance_controller.h @@ -17,9 +17,12 @@ #include #include #include +#include #include #include +#include #include +#include // Flexiv @@ -29,8 +32,15 @@ class JointImpedanceController : public controller_interface::Controller< hardware_interface::EffortJointInterface> { public: - // JointImpedanceController(); - // ~JointImpedanceController(); + struct Commands + { + std::array positions_; // Last commanded position + std::array velocities_; // Last commanded velocity + bool has_velocity_; // false if no velocity command has been specified + }; + + JointImpedanceController() = default; + ~JointImpedanceController(); bool init( hardware_interface::EffortJointInterface* hw, ros::NodeHandle& nh); @@ -39,16 +49,26 @@ class JointImpedanceController : public controller_interface::Controller< void starting(const ros::Time& time); - void setCommand(double pos_command); + void setCommand(std::array pos_command); - void setCommand(double pos_command, double vel_command); + void setCommand( + std::array pos_command, std::array vel_command); private: std::vector joints_; + std::vector joints_urdf_; std::vector k_p_; std::vector k_d_; - std::array q_init_; static const size_t num_joints_ = 7; + + // Commands + realtime_tools::RealtimeBuffer command_; + Commands command_struct_; + ros::Subscriber sub_command_; + + void setCommandCB(const flexiv_msgs::JointPosVelConstPtr& msg); + + void enforceJointLimits(int joint_index, double& pos_command); }; } // namespace flexiv_controllers diff --git a/flexiv_controllers/src/joint_impedance_controller.cpp b/flexiv_controllers/src/joint_impedance_controller.cpp index 3c12328..5808aef 100644 --- a/flexiv_controllers/src/joint_impedance_controller.cpp +++ b/flexiv_controllers/src/joint_impedance_controller.cpp @@ -3,6 +3,11 @@ namespace flexiv_controllers { +JointImpedanceController::~JointImpedanceController() +{ + sub_command_.shutdown(); +} + bool JointImpedanceController::init( hardware_interface::EffortJointInterface* hw, ros::NodeHandle& nh) { @@ -42,6 +47,10 @@ bool JointImpedanceController::init( } } + // Start command subscriber + sub_command_ = nh.subscribe( + "command", 1, &JointImpedanceController::setCommandCB, this); + // Get joint handles from hardware interface joints_.resize(num_joints_); for (std::size_t i = 0; i < num_joints_; ++i) { @@ -55,32 +64,128 @@ bool JointImpedanceController::init( } } + // Get URDF info about joints + urdf::Model urdf; + if (!urdf.initParamWithNodeHandle("robot_description", nh)) { + ROS_ERROR("JointImpedanceController: Failed to parse urdf file"); + return false; + } + joints_urdf_.resize(num_joints_); + for (std::size_t i = 0; i < num_joints_; ++i) { + joints_urdf_[i] = urdf.getJoint(joint_names[i]); + if (!joints_urdf_[i]) { + ROS_ERROR( + "JointImpedanceController: Could not find joint '%s' in urdf", + joint_names[i].c_str()); + return false; + } + } + return true; } +void JointImpedanceController::setCommand(std::array pos_command) +{ + command_struct_.has_velocity_ = false; + for (std::size_t i = 0; i < num_joints_; ++i) { + command_struct_.positions_[i] = pos_command[i]; + } + + command_.writeFromNonRT(command_struct_); +} + +void JointImpedanceController::setCommand( + std::array pos_command, std::array vel_command) +{ + command_struct_.has_velocity_ = true; + for (std::size_t i = 0; i < num_joints_; ++i) { + command_struct_.positions_[i] = pos_command[i]; + command_struct_.velocities_[i] = pos_command[i]; + } + + command_.writeFromNonRT(command_struct_); +} + void JointImpedanceController::starting(const ros::Time& /*time*/) { + std::array pos_command; + for (std::size_t i = 0; i < num_joints_; ++i) { - q_init_[i] = joints_[i].getPosition(); + pos_command[i] = joints_[i].getPosition(); + enforceJointLimits(i, pos_command[i]); + command_struct_.positions_[i] = pos_command[i]; } + command_struct_.has_velocity_ = false; + + command_.initRT(command_struct_); } void JointImpedanceController::update( const ros::Time& /*time*/, const ros::Duration& /*period*/) { - std::array q_des = q_init_; + command_struct_ = *(command_.readFromRT()); for (std::size_t i = 0; i < num_joints_; ++i) { // Get current joint positions and velocities double q = joints_[i].getPosition(); double dq = joints_[i].getVelocity(); + // Get target positions and velocities + double q_des = command_struct_.positions_[i]; + double dq_des = 0; + if (command_struct_.has_velocity_) { + dq_des = command_struct_.velocities_[i]; + } + + enforceJointLimits(i, q_des); + // Compute torque - double tau = k_p_[i] * (q_des[i] - q) - k_d_[i] * (dq); + double tau = k_p_[i] * (q_des - q) + k_d_[i] * (dq_des - dq); joints_[i].setCommand(tau); } } +void JointImpedanceController::setCommandCB( + const flexiv_msgs::JointPosVelConstPtr& msg) +{ + std::array command_positions; + std::array command_velocities; + if (msg->positions.size() == num_joints_ + && msg->velocities.size() == num_joints_) { + memcpy( + &command_positions, &msg->positions[0], sizeof(command_positions)); + memcpy(&command_velocities, &msg->velocities[0], + sizeof(command_velocities)); + setCommand(command_positions, command_velocities); + } else if (msg->positions.size() == num_joints_ + && msg->velocities.size() != num_joints_) { + memcpy( + &command_positions, &msg->positions[0], sizeof(command_positions)); + setCommand(command_positions); + } else { + ROS_ERROR( + "JointImpedanceController: Invalid command message received!"); + } +} + +void JointImpedanceController::enforceJointLimits( + int joint_index, double& pos_command) +{ + if (joints_urdf_[joint_index]->type == urdf::Joint::REVOLUTE + || joints_urdf_[joint_index]->type == urdf::Joint::PRISMATIC) { + if (pos_command + > joints_urdf_[joint_index]->limits->upper) // above upper limnit + { + pos_command = joints_urdf_[joint_index]->limits->upper; + } else if (pos_command < joints_urdf_[joint_index] + ->limits->lower) // below lower limit + { + pos_command = joints_urdf_[joint_index]->limits->lower; + } + } +} + } // namespace flexiv_controllers -PLUGINLIB_EXPORT_CLASS(flexiv_controllers::JointImpedanceController, controller_interface::ControllerBase) +PLUGINLIB_EXPORT_CLASS(flexiv_controllers::JointImpedanceController, + controller_interface::ControllerBase)