diff --git a/flexiv_bringup/config/rizon_controllers.yaml b/flexiv_bringup/config/rizon_controllers.yaml index 5b56797..a537d02 100644 --- a/flexiv_bringup/config/rizon_controllers.yaml +++ b/flexiv_bringup/config/rizon_controllers.yaml @@ -22,7 +22,7 @@ position_joint_trajectory_controller: joints: *robot_joints constraints: goal_time: 0.5 - stopped_velocity_tolerance: 0.05 + stopped_velocity_tolerance: 0.01 joint1: { goal: 0.05} joint2: { goal: 0.05} joint3: { goal: 0.05} @@ -33,53 +33,3 @@ position_joint_trajectory_controller: stop_trajectory_duration: 0.5 state_publish_rate: 100.0 action_monitor_rate: 20.0 - -velocity_joint_trajectory_controller: - type: velocity_controllers/JointTrajectoryController - joints: *robot_joints - constraints: - goal_time: 0.5 - stopped_velocity_tolerance: 0.05 - joint1: { goal: 0.05} - joint2: { goal: 0.05} - joint3: { goal: 0.05} - joint4: { goal: 0.05} - joint5: { goal: 0.05} - joint6: { goal: 0.05} - joint7: { goal: 0.05} - gains: - joint1: {p: 5.0, i: 0.05, d: 0.1, i_clamp: 1} - joint2: {p: 5.0, i: 0.05, d: 0.1, i_clamp: 1} - joint3: {p: 5.0, i: 0.05, d: 0.1, i_clamp: 1} - joint4: {p: 5.0, i: 0.05, d: 0.1, i_clamp: 1} - joint5: {p: 5.0, i: 0.05, d: 0.1, i_clamp: 1} - joint6: {p: 5.0, i: 0.05, d: 0.1, i_clamp: 1} - joint7: {p: 5.0, i: 0.05, d: 0.1, i_clamp: 1} - stop_trajectory_duration: 0.5 - state_publish_rate: 100.0 - action_monitor_rate: 20.0 - -effort_joint_trajectory_controller: - type: effort_controllers/JointTrajectoryController - joints: *robot_joints - constraints: - goal_time: 0.5 - stopped_velocity_tolerance: 0.05 - joint1: { goal: 0.05} - joint2: { goal: 0.05} - joint3: { goal: 0.05} - joint4: { goal: 0.05} - joint5: { goal: 0.05} - joint6: { goal: 0.05} - joint7: { goal: 0.05} - gains: - joint1: { p: 3000, d: 80, i: 0 } - joint2: { p: 3000, d: 80, i: 0 } - joint3: { p: 800, d: 40, i: 0 } - joint4: { p: 800, d: 40, i: 0 } - joint5: { p: 200, d: 8, i: 0 } - joint6: { p: 200, d: 8, i: 0 } - joint7: { p: 200, d: 8, i: 0 } - stop_trajectory_duration: 0.5 - state_publish_rate: 100.0 - action_monitor_rate: 20.0 diff --git a/flexiv_hardware/include/flexiv_hardware/flexiv_hardware_interface.h b/flexiv_hardware/include/flexiv_hardware/flexiv_hardware_interface.h index 64b7318..4819b07 100644 --- a/flexiv_hardware/include/flexiv_hardware/flexiv_hardware_interface.h +++ b/flexiv_hardware/include/flexiv_hardware/flexiv_hardware_interface.h @@ -38,7 +38,6 @@ class FlexivHardwareInterface : public hardware_interface::RobotHW public: /** * @brief Construct a new FlexivHardwareInterface object - * */ FlexivHardwareInterface(); @@ -48,7 +47,6 @@ class FlexivHardwareInterface : public hardware_interface::RobotHW * @brief Handles the setup functionality for the ROS interface. This * includes parsing ROS parameters, connecting to the robot, and setting up * hardware interfaces for ros_control. - * * @param[in] root_nh Root level ROS node handle. * @param[in] robot_hw_nh ROS node handle for the robot namespace. * @return True if successful, false otherwise. @@ -59,7 +57,6 @@ class FlexivHardwareInterface : public hardware_interface::RobotHW /** * @brief Reads the parameterization of the hardware class from the ROS * parameter server (e.g. local_ip, robot_ip, joint_names etc.) - * * @param[in] root_nh A node handle in the root namespace of the control * node. * @param[in] robot_hw_nh A node handle in the namespace of the robot @@ -71,7 +68,6 @@ class FlexivHardwareInterface : public hardware_interface::RobotHW /** * @brief Initializes the class in terms of ros_control interfaces. - * * @note You have to call initParameters beforehand. Use the complete * initialization routine \ref init() method to control robots. * @param[in] robot_hw_nh A node handle in the namespace of the robot @@ -81,7 +77,6 @@ class FlexivHardwareInterface : public hardware_interface::RobotHW /** * @brief Reads data from the robot. - * * @param[in] time Current time. * @param[in] period Duration of current control loop iteration. */ @@ -90,7 +85,6 @@ class FlexivHardwareInterface : public hardware_interface::RobotHW /** * @brief Writes data to the robot. - * * @param[in] time Current time. * @param[in] period Duration of current control loop iteration. */ @@ -99,13 +93,11 @@ class FlexivHardwareInterface : public hardware_interface::RobotHW /** * @brief Set all members to default values. - * */ virtual void reset(); /** * @brief Prepares switching between controllers. - * * @param[in] start_list List of requested controllers to start. * @param[in] stop_list List of requested controllers to stop. * @return True if the preparation has been successful, false otherwise. @@ -117,7 +109,6 @@ class FlexivHardwareInterface : public hardware_interface::RobotHW /** * @brief Perform controllers switching. - * * @param[in] start_list List of requested controllers to start. * @param[in] stop_list List of requested controllers to stop. */ @@ -126,8 +117,8 @@ class FlexivHardwareInterface : public hardware_interface::RobotHW const std::list& stop_list) override; - /** @brief Enforce limits on position, velocity, and effort. - * + /** + * @brief Enforce limits on position, velocity, and effort. * @param[in] period The duration of the current cycle. */ virtual void enforceLimits(const ros::Duration& period); @@ -135,7 +126,6 @@ class FlexivHardwareInterface : public hardware_interface::RobotHW protected: /** * @brief Uses the robot_ip to connect to the robot via RDK. - * * @return True if successful, false otherwise. */ virtual bool initRobot(); @@ -143,14 +133,27 @@ class FlexivHardwareInterface : public hardware_interface::RobotHW /** * @brief Get current joint position and set the joint command with those * values. - * */ virtual void setInitPosition(); + /** + * @brief Checks whether a vector of doubles contains NaN values. + * @param[in] vector The vector to check. + * @return True if the vector contains NaN values, false otherwise. + */ + static bool vectorHasNan(const std::vector& vector) + { + for (const double& value : vector) { + if (std::isnan(value)) { + return true; + } + } + return false; + } + /** * @brief Configures a limit interface to enforce limits on effort, velocity * or position level on joint commands. - * * @param[in] limits_interface Limit interface for the robot. * @param[in] command_interface Command interface to match with the limit * interface. @@ -198,7 +201,6 @@ class FlexivHardwareInterface : public hardware_interface::RobotHW /** * @brief Checks whether a resource list contains joints from this hardware * interface. - * * @param[in] claimed_resources List of claimed resources. * @return True if the list contains joints from this hardware interface, * false otherwise. @@ -209,7 +211,6 @@ class FlexivHardwareInterface : public hardware_interface::RobotHW /** * @brief Publishes external force applied on TCP in TCP frame \f$ * ^{TCP}F_{ext}~[N][Nm] \f$ and base frame \f$ ^{0}F_{ext}~[N][Nm] \f$. - * */ virtual void publishExternalForce(); @@ -239,6 +240,7 @@ class FlexivHardwareInterface : public hardware_interface::RobotHW std::vector joint_position_state_; std::vector joint_velocity_state_; std::vector joint_effort_state_; + // External force std::vector ext_force_in_tcp_; std::vector ext_force_in_base_; @@ -250,10 +252,10 @@ class FlexivHardwareInterface : public hardware_interface::RobotHW std::vector internal_joint_position_command_; // Controller - bool position_controller_running_; - bool velocity_controller_running_; - bool effort_controller_running_; - bool controllers_initialized_; + std::atomic position_controller_running_; + std::atomic velocity_controller_running_; + std::atomic effort_controller_running_; + std::atomic controllers_initialized_; // Publisher std::unique_ptr< diff --git a/flexiv_hardware/src/flexiv_hardware_interface.cpp b/flexiv_hardware/src/flexiv_hardware_interface.cpp index c59f992..e68e18a 100644 --- a/flexiv_hardware/src/flexiv_hardware_interface.cpp +++ b/flexiv_hardware/src/flexiv_hardware_interface.cpp @@ -14,32 +14,26 @@ #include "flexiv_hardware/flexiv_hardware_interface.h" namespace flexiv_hardware { -FlexivHardwareInterface::FlexivHardwareInterface() { } +FlexivHardwareInterface::FlexivHardwareInterface() +: joint_position_state_({0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0}) +, joint_velocity_state_({0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0}) +, joint_effort_state_({0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0}) +, joint_position_command_({0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0}) +, joint_velocity_command_({0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0}) +, joint_effort_command_({0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0}) +, internal_joint_position_command_({0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0}) +, ext_force_in_tcp_({0.0, 0.0, 0.0, 0.0, 0.0, 0.0}) +, ext_force_in_base_({0.0, 0.0, 0.0, 0.0, 0.0, 0.0}) +, position_controller_running_(false) +, velocity_controller_running_(false) +, effort_controller_running_(false) +, controllers_initialized_(false) +{ +} bool FlexivHardwareInterface::init( ros::NodeHandle& root_nh, ros::NodeHandle& robot_hw_nh) { - // States - joint_position_state_.resize(num_joints_, 0.0); - joint_velocity_state_.resize(num_joints_, 0.0); - joint_effort_state_.resize(num_joints_, 0.0); - - // External force - ext_force_in_tcp_.resize(6, 0.0); - ext_force_in_base_.resize(6, 0.0); - - // Commands - joint_position_command_.resize(num_joints_, 0.0); - joint_velocity_command_.resize(num_joints_, 0.0); - joint_effort_command_.resize(num_joints_, 0.0); - internal_joint_position_command_.resize(num_joints_, 0.0); - - // Controller - position_controller_running_ = false; - velocity_controller_running_ = false; - effort_controller_running_ = false; - controllers_initialized_ = false; - if (!initParameters(root_nh, robot_hw_nh)) { ROS_ERROR("Failed to parse all required parameters."); return false; @@ -131,6 +125,7 @@ void FlexivHardwareInterface::initROSInterfaces( setupLimitInterface( effort_joint_limit_interface_, effort_joint_interface_); + // Register interfaces registerInterface(&joint_state_interface_); // From RobotHW base class. registerInterface(&position_joint_interface_); // From RobotHW base class. registerInterface(&velocity_joint_interface_); // From RobotHW base class. @@ -162,9 +157,6 @@ bool FlexivHardwareInterface::initRobot() } ROS_INFO("Robot is now operational."); - // get current position and set to initial position - setInitPosition(); - return true; } @@ -236,20 +228,23 @@ void FlexivHardwareInterface::write( std::fill(target_velocity.begin(), target_velocity.end(), 0.0); if (position_controller_running_ - && robot_->getMode() == flexiv::MODE_JOINT_POSITION) { + && robot_->getMode() == flexiv::MODE_JOINT_POSITION + && !(vectorHasNan(joint_position_command_))) { robot_->streamJointPosition( joint_position_command_, target_velocity, target_acceleration); } else if (velocity_controller_running_ - && robot_->getMode() == flexiv::MODE_JOINT_POSITION) { + && robot_->getMode() == flexiv::MODE_JOINT_POSITION + && !(vectorHasNan(joint_velocity_command_))) { for (std::size_t i = 0; i < num_joints_; i++) { internal_joint_position_command_[i] += joint_velocity_command_[i] * period.toSec(); } robot_->streamJointPosition(internal_joint_position_command_, - target_velocity, target_acceleration); + joint_velocity_command_, target_acceleration); } else if (effort_controller_running_ - && robot_->getMode() == flexiv::MODE_JOINT_TORQUE) { - robot_->streamJointTorque(joint_effort_command_); + && robot_->getMode() == flexiv::MODE_JOINT_TORQUE + && !(vectorHasNan(joint_effort_command_))) { + robot_->streamJointTorque(joint_effort_command_, true, true); } } @@ -258,12 +253,12 @@ void FlexivHardwareInterface::setInitPosition() flexiv::RobotStates robot_states; // Read the current joint positions - if (robot_->isOperational()) { + if (robot_->isOperational() && robot_->getMode() == flexiv::MODE_IDLE) { robot_->getRobotStates(robot_states); joint_position_state_ = robot_states.q; } - // set the initial joint position as command + // Set the initial joint position as command joint_position_command_ = joint_position_state_; } @@ -319,12 +314,15 @@ void FlexivHardwareInterface::doSwitch( if (resource_it.hardware_interface == "hardware_interface::PositionJointInterface") { position_controller_running_ = false; + robot_->stop(); } else if (resource_it.hardware_interface == "hardware_interface::VelocityJointInterface") { velocity_controller_running_ = false; + robot_->stop(); } else if (resource_it.hardware_interface == "hardware_interface::EffortJointInterface") { effort_controller_running_ = false; + robot_->stop(); } } } @@ -336,26 +334,48 @@ void FlexivHardwareInterface::doSwitch( == "hardware_interface::PositionJointInterface") { velocity_controller_running_ = false; effort_controller_running_ = false; - position_controller_running_ = true; - if (robot_->getMode() != flexiv::MODE_JOINT_POSITION) { - robot_->setMode(flexiv::MODE_JOINT_POSITION); + + // get current position and set to initial position + setInitPosition(); + + // Set to joint position mode + robot_->setMode(flexiv::MODE_JOINT_POSITION); + + // Wait for the mode to be switched + while (robot_->getMode() != flexiv::MODE_JOINT_POSITION) { + std::this_thread::sleep_for( + std::chrono::milliseconds(1)); } + + position_controller_running_ = true; } else if (resource_it.hardware_interface == "hardware_interface::VelocityJointInterface") { position_controller_running_ = false; effort_controller_running_ = false; - velocity_controller_running_ = true; - if (robot_->getMode() != flexiv::MODE_JOINT_POSITION) { - robot_->setMode(flexiv::MODE_JOINT_POSITION); + + // Set to joint position mode + robot_->setMode(flexiv::MODE_JOINT_POSITION); + + // Wait for the mode to be switched + while (robot_->getMode() != flexiv::MODE_JOINT_POSITION) { + std::this_thread::sleep_for( + std::chrono::milliseconds(1)); } + velocity_controller_running_ = true; } else if (resource_it.hardware_interface == "hardware_interface::EffortJointInterface") { position_controller_running_ = false; velocity_controller_running_ = false; - effort_controller_running_ = true; - if (robot_->getMode() != flexiv::MODE_JOINT_TORQUE) { - robot_->setMode(flexiv::MODE_JOINT_TORQUE); + + // Set to joint torque mode + robot_->setMode(flexiv::MODE_JOINT_TORQUE); + + // Wait for the mode to be switched + while (robot_->getMode() != flexiv::MODE_JOINT_TORQUE) { + std::this_thread::sleep_for( + std::chrono::milliseconds(1)); } + effort_controller_running_ = true; } } } diff --git a/flexiv_hardware/src/flexiv_hardware_interface_node.cpp b/flexiv_hardware/src/flexiv_hardware_interface_node.cpp index 1cc0cbb..bf83381 100644 --- a/flexiv_hardware/src/flexiv_hardware_interface_node.cpp +++ b/flexiv_hardware/src/flexiv_hardware_interface_node.cpp @@ -35,6 +35,7 @@ int main(int argc, char** argv) ROS_ERROR("Failed to initialize Flexiv Hardware Interface."); exit(1); } + // Create the controller manager controller_manager::ControllerManager cm( flexiv_hardware_interface.get(), nh);