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 new file mode 100644 index 0000000..0fdf07f --- /dev/null +++ b/flexiv_controllers/CMakeLists.txt @@ -0,0 +1,65 @@ +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 + controller_manager + flexiv_msgs + hardware_interface + joint_limits_interface + pluginlib + realtime_tools + roscpp + urdf +) + +catkin_package( + INCLUDE_DIRS + include + CATKIN_DEPENDS + controller_interface + controller_manager + flexiv_msgs + hardware_interface + joint_limits_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 targets +install(TARGETS ${PROJECT_NAME} + ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +) + +# Install headers +install(DIRECTORY include/${PROJECT_NAME}/ + DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} +) + +# Install plugins +install(FILES flexiv_controllers_plugin.xml + DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} +) diff --git a/flexiv_controllers/flexiv_controllers_plugin.xml b/flexiv_controllers/flexiv_controllers_plugin.xml new file mode 100644 index 0000000..bc96b21 --- /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. + + + \ No newline at end of file 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..41daa73 --- /dev/null +++ b/flexiv_controllers/include/flexiv_controllers/joint_impedance_controller.h @@ -0,0 +1,76 @@ +/** + * @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 +#include +#include +#include + +// Flexiv + +namespace flexiv_controllers { + +class JointImpedanceController : public controller_interface::Controller< + hardware_interface::EffortJointInterface> +{ +public: + 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); + + void update(const ros::Time& time, const ros::Duration& period); + + void starting(const ros::Time& time); + + void setCommand(std::array pos_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_; + 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 + +#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..2de1775 --- /dev/null +++ b/flexiv_controllers/package.xml @@ -0,0 +1,35 @@ + + + flexiv_controllers + 0.7.0 + Flexiv custom ROS controllers + Mun Seng Phoon + Apache License 2.0 + Mun Seng Phoon + + catkin + + controller_interface + controller_manager + hardware_interface + joint_limits_interface + pluginlib + realtime_tools + roscpp + urdf + flexiv_msgs + + controller_interface + controller_manager + hardware_interface + joint_limits_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..5808aef --- /dev/null +++ b/flexiv_controllers/src/joint_impedance_controller.cpp @@ -0,0 +1,191 @@ +#include +#include + +namespace flexiv_controllers { + +JointImpedanceController::~JointImpedanceController() +{ + sub_command_.shutdown(); +} + +bool JointImpedanceController::init( + hardware_interface::EffortJointInterface* hw, ros::NodeHandle& nh) +{ + // Get joint name from parameter server + std::vector joint_names; + if (!nh.getParam("joints", 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; + } + } + + // 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) { + 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; + } + } + + // 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) { + 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*/) +{ + 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 - 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) 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);