From f1870c945b7a431f7826c03266210eceeda9a30d Mon Sep 17 00:00:00 2001 From: munseng Date: Tue, 21 Feb 2023 17:24:52 +0800 Subject: [PATCH 1/3] Remove unused controllers config --- flexiv_bringup/config/rizon_controllers.yaml | 51 -------------------- 1 file changed, 51 deletions(-) diff --git a/flexiv_bringup/config/rizon_controllers.yaml b/flexiv_bringup/config/rizon_controllers.yaml index 5b56797..ef752df 100644 --- a/flexiv_bringup/config/rizon_controllers.yaml +++ b/flexiv_bringup/config/rizon_controllers.yaml @@ -22,7 +22,6 @@ position_joint_trajectory_controller: 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} @@ -33,53 +32,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 From 8f3dbd60587177adfa713333a7ef6411449bf1f2 Mon Sep 17 00:00:00 2001 From: munseng Date: Tue, 21 Feb 2023 17:27:00 +0800 Subject: [PATCH 2/3] Hold joints before user command arrives --- .../flexiv_hardware_interface.h | 25 +--- .../src/flexiv_hardware_interface.cpp | 119 ++++++++++++------ .../src/flexiv_hardware_interface_node.cpp | 1 + 3 files changed, 83 insertions(+), 62 deletions(-) diff --git a/flexiv_hardware/include/flexiv_hardware/flexiv_hardware_interface.h b/flexiv_hardware/include/flexiv_hardware/flexiv_hardware_interface.h index 64b7318..222f008 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,22 +126,13 @@ 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(); - /** - * @brief Get current joint position and set the joint command with those - * values. - * - */ - virtual void setInitPosition(); - /** * @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 +180,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 +190,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 +219,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_; diff --git a/flexiv_hardware/src/flexiv_hardware_interface.cpp b/flexiv_hardware/src/flexiv_hardware_interface.cpp index c59f992..1550a53 100644 --- a/flexiv_hardware/src/flexiv_hardware_interface.cpp +++ b/flexiv_hardware/src/flexiv_hardware_interface.cpp @@ -20,19 +20,26 @@ 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); + joint_position_state_.resize( + num_joints_, std::numeric_limits::quiet_NaN()); + joint_velocity_state_.resize( + num_joints_, std::numeric_limits::quiet_NaN()); + joint_effort_state_.resize( + num_joints_, std::numeric_limits::quiet_NaN()); // External force - ext_force_in_tcp_.resize(6, 0.0); - ext_force_in_base_.resize(6, 0.0); + ext_force_in_tcp_.resize(6, std::numeric_limits::quiet_NaN()); + ext_force_in_base_.resize(6, std::numeric_limits::quiet_NaN()); // 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); + joint_position_command_.resize( + num_joints_, std::numeric_limits::quiet_NaN()); + joint_velocity_command_.resize( + num_joints_, std::numeric_limits::quiet_NaN()); + joint_effort_command_.resize( + num_joints_, std::numeric_limits::quiet_NaN()); + internal_joint_position_command_.resize( + num_joints_, std::numeric_limits::quiet_NaN()); // Controller position_controller_running_ = false; @@ -162,9 +169,6 @@ bool FlexivHardwareInterface::initRobot() } ROS_INFO("Robot is now operational."); - // get current position and set to initial position - setInitPosition(); - return true; } @@ -235,36 +239,38 @@ void FlexivHardwareInterface::write( std::fill(target_acceleration.begin(), target_acceleration.end(), 0.0); std::fill(target_velocity.begin(), target_velocity.end(), 0.0); + bool isNanPos = false; + bool isNanVel = false; + bool isNanEff = false; + for (std::size_t i = 0; i < num_joints_; i++) { + if (joint_position_command_[i] != joint_position_command_[i]) { + isNanPos = true; + } + if (joint_velocity_command_[i] != joint_velocity_command_[i]) { + isNanVel = true; + } + if (joint_effort_command_[i] != joint_effort_command_[i]) { + isNanEff = true; + } + } + if (position_controller_running_ - && robot_->getMode() == flexiv::MODE_JOINT_POSITION) { + && robot_->getMode() == flexiv::MODE_JOINT_POSITION && !isNanPos) { 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 + && !isNanVel) { 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_); - } -} - -void FlexivHardwareInterface::setInitPosition() -{ - flexiv::RobotStates robot_states; - - // Read the current joint positions - if (robot_->isOperational()) { - robot_->getRobotStates(robot_states); - joint_position_state_ = robot_states.q; + && robot_->getMode() == flexiv::MODE_JOINT_TORQUE && !isNanEff) { + robot_->streamJointTorque(joint_effort_command_, true, true); } - - // set the initial joint position as command - joint_position_command_ = joint_position_state_; } void FlexivHardwareInterface::reset() @@ -319,12 +325,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 +345,56 @@ 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); + // Hold joints before user commands arrives + std::fill(joint_position_command_.begin(), + joint_position_command_.end(), + std::numeric_limits::quiet_NaN()); + + // 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); + // Hold joints before user commands arrives + std::fill(joint_velocity_command_.begin(), + joint_velocity_command_.end(), + std::numeric_limits::quiet_NaN()); + + // 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); + // Hold joints before user commands arrives + std::fill(joint_effort_command_.begin(), + joint_effort_command_.end(), + std::numeric_limits::quiet_NaN()); + + // 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); From 69c737179d601f7dcd41942cd5aa71cfca9cbfcb Mon Sep 17 00:00:00 2001 From: munseng Date: Wed, 22 Feb 2023 14:58:21 +0800 Subject: [PATCH 3/3] Update joint interface to set init positions --- flexiv_bringup/config/rizon_controllers.yaml | 1 + .../flexiv_hardware_interface.h | 29 +++++- .../src/flexiv_hardware_interface.cpp | 99 ++++++++----------- 3 files changed, 66 insertions(+), 63 deletions(-) diff --git a/flexiv_bringup/config/rizon_controllers.yaml b/flexiv_bringup/config/rizon_controllers.yaml index ef752df..a537d02 100644 --- a/flexiv_bringup/config/rizon_controllers.yaml +++ b/flexiv_bringup/config/rizon_controllers.yaml @@ -22,6 +22,7 @@ position_joint_trajectory_controller: joints: *robot_joints constraints: goal_time: 0.5 + stopped_velocity_tolerance: 0.01 joint1: { goal: 0.05} joint2: { goal: 0.05} joint3: { goal: 0.05} diff --git a/flexiv_hardware/include/flexiv_hardware/flexiv_hardware_interface.h b/flexiv_hardware/include/flexiv_hardware/flexiv_hardware_interface.h index 222f008..4819b07 100644 --- a/flexiv_hardware/include/flexiv_hardware/flexiv_hardware_interface.h +++ b/flexiv_hardware/include/flexiv_hardware/flexiv_hardware_interface.h @@ -130,6 +130,27 @@ class FlexivHardwareInterface : public hardware_interface::RobotHW */ virtual bool initRobot(); + /** + * @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. @@ -231,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 1550a53..e68e18a 100644 --- a/flexiv_hardware/src/flexiv_hardware_interface.cpp +++ b/flexiv_hardware/src/flexiv_hardware_interface.cpp @@ -14,39 +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_, std::numeric_limits::quiet_NaN()); - joint_velocity_state_.resize( - num_joints_, std::numeric_limits::quiet_NaN()); - joint_effort_state_.resize( - num_joints_, std::numeric_limits::quiet_NaN()); - - // External force - ext_force_in_tcp_.resize(6, std::numeric_limits::quiet_NaN()); - ext_force_in_base_.resize(6, std::numeric_limits::quiet_NaN()); - - // Commands - joint_position_command_.resize( - num_joints_, std::numeric_limits::quiet_NaN()); - joint_velocity_command_.resize( - num_joints_, std::numeric_limits::quiet_NaN()); - joint_effort_command_.resize( - num_joints_, std::numeric_limits::quiet_NaN()); - internal_joint_position_command_.resize( - num_joints_, std::numeric_limits::quiet_NaN()); - - // 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; @@ -138,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. @@ -239,28 +227,14 @@ void FlexivHardwareInterface::write( std::fill(target_acceleration.begin(), target_acceleration.end(), 0.0); std::fill(target_velocity.begin(), target_velocity.end(), 0.0); - bool isNanPos = false; - bool isNanVel = false; - bool isNanEff = false; - for (std::size_t i = 0; i < num_joints_; i++) { - if (joint_position_command_[i] != joint_position_command_[i]) { - isNanPos = true; - } - if (joint_velocity_command_[i] != joint_velocity_command_[i]) { - isNanVel = true; - } - if (joint_effort_command_[i] != joint_effort_command_[i]) { - isNanEff = true; - } - } - if (position_controller_running_ - && robot_->getMode() == flexiv::MODE_JOINT_POSITION && !isNanPos) { + && 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 - && !isNanVel) { + && !(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(); @@ -268,11 +242,26 @@ void FlexivHardwareInterface::write( robot_->streamJointPosition(internal_joint_position_command_, joint_velocity_command_, target_acceleration); } else if (effort_controller_running_ - && robot_->getMode() == flexiv::MODE_JOINT_TORQUE && !isNanEff) { + && robot_->getMode() == flexiv::MODE_JOINT_TORQUE + && !(vectorHasNan(joint_effort_command_))) { robot_->streamJointTorque(joint_effort_command_, true, true); } } +void FlexivHardwareInterface::setInitPosition() +{ + flexiv::RobotStates robot_states; + + // Read the current joint positions + 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 + joint_position_command_ = joint_position_state_; +} + void FlexivHardwareInterface::reset() { // Reset joint limits state, in case of mode switch or e-stop @@ -345,10 +334,9 @@ void FlexivHardwareInterface::doSwitch( == "hardware_interface::PositionJointInterface") { velocity_controller_running_ = false; effort_controller_running_ = false; - // Hold joints before user commands arrives - std::fill(joint_position_command_.begin(), - joint_position_command_.end(), - std::numeric_limits::quiet_NaN()); + + // get current position and set to initial position + setInitPosition(); // Set to joint position mode robot_->setMode(flexiv::MODE_JOINT_POSITION); @@ -358,15 +346,12 @@ void FlexivHardwareInterface::doSwitch( 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; - // Hold joints before user commands arrives - std::fill(joint_velocity_command_.begin(), - joint_velocity_command_.end(), - std::numeric_limits::quiet_NaN()); // Set to joint position mode robot_->setMode(flexiv::MODE_JOINT_POSITION); @@ -381,10 +366,6 @@ void FlexivHardwareInterface::doSwitch( == "hardware_interface::EffortJointInterface") { position_controller_running_ = false; velocity_controller_running_ = false; - // Hold joints before user commands arrives - std::fill(joint_effort_command_.begin(), - joint_effort_command_.end(), - std::numeric_limits::quiet_NaN()); // Set to joint torque mode robot_->setMode(flexiv::MODE_JOINT_TORQUE);