From bd03350cc81c28078c05cd9264c3f036488e3e56 Mon Sep 17 00:00:00 2001 From: Kayman Date: Wed, 28 Aug 2019 09:42:39 +0900 Subject: [PATCH] Fixed body splashing when changing module between tuning_module and others. --- .../robotis_controller/robotis_controller.h | 3 +- .../robotis_controller/robotis_controller.cpp | 76 ++++++++----------- 2 files changed, 34 insertions(+), 45 deletions(-) diff --git a/robotis_controller/include/robotis_controller/robotis_controller.h b/robotis_controller/include/robotis_controller/robotis_controller.h index 4772f78..05ae93c 100755 --- a/robotis_controller/include/robotis_controller/robotis_controller.h +++ b/robotis_controller/include/robotis_controller/robotis_controller.h @@ -67,6 +67,7 @@ private: bool init_pose_loaded_; bool is_timer_running_; bool is_offset_enabled_; + double offset_ratio_; bool stop_timer_; pthread_t timer_thread_; ControllerMode controller_mode_; @@ -86,7 +87,7 @@ private: void initializeSyncWrite(); public: - const int NONE_GAIN = -1; + const int NONE_GAIN = 65535; bool DEBUG_PRINT; Robot *robot_; diff --git a/robotis_controller/src/robotis_controller/robotis_controller.cpp b/robotis_controller/src/robotis_controller/robotis_controller.cpp index c91d696..548bfaa 100755 --- a/robotis_controller/src/robotis_controller/robotis_controller.cpp +++ b/robotis_controller/src/robotis_controller/robotis_controller.cpp @@ -31,6 +31,7 @@ using namespace robotis_framework; RobotisController::RobotisController() : is_timer_running_(false), is_offset_enabled_(true), + offset_ratio_(1.0), stop_timer_(false), init_pose_loaded_(false), timer_thread_(0), @@ -143,10 +144,7 @@ void RobotisController::initializeSyncWrite() if ((dxl->present_position_item_ != 0) && (dxl->bulk_read_items_[i]->item_name_ == dxl->present_position_item_->item_name_)) { - if(is_offset_enabled_) - dxl->dxl_state_->present_position_ = dxl->convertValue2Radian(read_data) - dxl->dxl_state_->position_offset_; // remove offset - else - dxl->dxl_state_->present_position_ = dxl->convertValue2Radian(read_data); + dxl->dxl_state_->present_position_ = dxl->convertValue2Radian(read_data) - dxl->dxl_state_->position_offset_ * offset_ratio_; dxl->dxl_state_->goal_position_ = dxl->dxl_state_->present_position_; port_to_sync_write_position_[dxl->port_name_]->addParam(dxl->id_, sync_write_data); @@ -904,6 +902,22 @@ void RobotisController::process() is_process_running = true; // ROS_INFO("Controller::Process()"); + // offset ratio + if(is_offset_enabled_) + { + if(offset_ratio_ < 1.0) + offset_ratio_ += 0.01; + else + offset_ratio_ = 1.0; + } + else + { + if(offset_ratio_ > 0.0) + offset_ratio_ -= 0.01; + else + offset_ratio_ = 0.0; + } + ros::Time start_time; ros::Duration time_duration; @@ -959,10 +973,7 @@ void RobotisController::process() // change dxl_state if (dxl->present_position_item_ != 0 && item->item_name_ == dxl->present_position_item_->item_name_) { - if(is_offset_enabled_) - dxl->dxl_state_->present_position_ = dxl->convertValue2Radian(data) - dxl->dxl_state_->position_offset_; // remove offset - else - dxl->dxl_state_->present_position_ = dxl->convertValue2Radian(data); + dxl->dxl_state_->present_position_ = dxl->convertValue2Radian(data) - dxl->dxl_state_->position_offset_ * offset_ratio_; } else if (dxl->present_velocity_item_ != 0 && item->item_name_ == dxl->present_velocity_item_->item_name_) dxl->dxl_state_->present_velocity_ = dxl->convertValue2Velocity(data); @@ -970,10 +981,7 @@ void RobotisController::process() dxl->dxl_state_->present_torque_ = dxl->convertValue2Torque(data); else if (dxl->goal_position_item_ != 0 && item->item_name_ == dxl->goal_position_item_->item_name_) { - if(is_offset_enabled_) - dxl->dxl_state_->goal_position_ = dxl->convertValue2Radian(data) - dxl->dxl_state_->position_offset_; // remove offset - else - dxl->dxl_state_->goal_position_ = dxl->convertValue2Radian(data); + dxl->dxl_state_->goal_position_ = dxl->convertValue2Radian(data) - dxl->dxl_state_->position_offset_ * offset_ratio_; } else if (dxl->goal_velocity_item_ != 0 && item->item_name_ == dxl->goal_velocity_item_->item_name_) dxl->dxl_state_->goal_velocity_ = dxl->convertValue2Velocity(data); @@ -1202,10 +1210,7 @@ void RobotisController::process() // change dxl_state if (dxl->present_position_item_ != 0 && item->item_name_ == dxl->present_position_item_->item_name_) { - if(is_offset_enabled_) - dxl->dxl_state_->present_position_ = dxl->convertValue2Radian(data) - dxl->dxl_state_->position_offset_; // remove offset - else - dxl->dxl_state_->present_position_ = dxl->convertValue2Radian(data); + dxl->dxl_state_->present_position_ = dxl->convertValue2Radian(data) - dxl->dxl_state_->position_offset_ * offset_ratio_; } else if (dxl->present_velocity_item_ != 0 && item->item_name_ == dxl->present_velocity_item_->item_name_) dxl->dxl_state_->present_velocity_ = dxl->convertValue2Velocity(data); @@ -1213,10 +1218,7 @@ void RobotisController::process() dxl->dxl_state_->present_torque_ = dxl->convertValue2Torque(data); else if (dxl->goal_position_item_ != 0 && item->item_name_ == dxl->goal_position_item_->item_name_) { - if(is_offset_enabled_) - dxl->dxl_state_->goal_position_ = dxl->convertValue2Radian(data) - dxl->dxl_state_->position_offset_; // remove offset - else - dxl->dxl_state_->goal_position_ = dxl->convertValue2Radian(data); + dxl->dxl_state_->goal_position_ = dxl->convertValue2Radian(data) - dxl->dxl_state_->position_offset_ * offset_ratio_; } else if (dxl->goal_velocity_item_ != 0 && item->item_name_ == dxl->goal_velocity_item_->item_name_) dxl->dxl_state_->goal_velocity_ = dxl->convertValue2Velocity(data); @@ -1321,10 +1323,7 @@ void RobotisController::process() { // add offset uint32_t pos_data; - if(is_offset_enabled_) - pos_data= dxl->convertRadian2Value(dxl_state->goal_position_ + dxl_state->position_offset_); - else - pos_data= dxl->convertRadian2Value(dxl_state->goal_position_); + pos_data= dxl->convertRadian2Value(dxl_state->goal_position_ + dxl_state->position_offset_ * offset_ratio_); uint8_t sync_write_data[4] = { 0 }; sync_write_data[0] = DXL_LOBYTE(DXL_LOWORD(pos_data)); @@ -1753,10 +1752,7 @@ void RobotisController::setJointStatesCallback(const sensor_msgs::JointState::Co { // add offset uint32_t pos_data; - if(is_offset_enabled_) - pos_data = dxl->convertRadian2Value(dxl->dxl_state_->goal_position_ + dxl->dxl_state_->position_offset_); - else - pos_data = dxl->convertRadian2Value(dxl->dxl_state_->goal_position_); + pos_data = dxl->convertRadian2Value(dxl->dxl_state_->goal_position_ + dxl->dxl_state_->position_offset_ * offset_ratio_); uint8_t sync_write_data[4] = { 0 }; sync_write_data[0] = DXL_LOBYTE(DXL_LOWORD(pos_data)); @@ -1804,6 +1800,10 @@ void RobotisController::setJointCtrlModuleCallback(const robotis_controller_msgs void RobotisController::enableOffsetCallback(const std_msgs::Bool::ConstPtr &msg) { is_offset_enabled_ = (bool)msg->data; + if(is_offset_enabled_) + offset_ratio_ = 0.0; + else + offset_ratio_ = 1.0; } bool RobotisController::getJointCtrlModuleService(robotis_controller_msgs::GetJointModule::Request &req, @@ -1938,10 +1938,7 @@ void RobotisController::setJointCtrlModuleThread(const robotis_controller_msgs:: continue; uint32_t _pos_data; - if(is_offset_enabled_) - _pos_data = _dxl->convertRadian2Value(_dxl->dxl_state_->goal_position_ + _dxl->dxl_state_->position_offset_); - else - _pos_data = _dxl->convertRadian2Value(_dxl->dxl_state_->goal_position_); + _pos_data = _dxl->convertRadian2Value(_dxl->dxl_state_->goal_position_ + _dxl->dxl_state_->position_offset_ * offset_ratio_); uint8_t _sync_write_data[4]; _sync_write_data[0] = DXL_LOBYTE(DXL_LOWORD(_pos_data)); @@ -1981,10 +1978,7 @@ void RobotisController::setJointCtrlModuleThread(const robotis_controller_msgs:: if(_mode == PositionControl) { uint32_t _pos_data; - if(is_offset_enabled_) - _pos_data = _dxl->convertRadian2Value(_dxl->dxl_state_->goal_position_ + _dxl->dxl_state_->position_offset_); - else - _pos_data = _dxl->convertRadian2Value(_dxl->dxl_state_->goal_position_); + _pos_data = _dxl->convertRadian2Value(_dxl->dxl_state_->goal_position_ + _dxl->dxl_state_->position_offset_ * offset_ratio_); uint8_t _sync_write_data[4]; _sync_write_data[0] = DXL_LOBYTE(DXL_LOWORD(_pos_data)); @@ -2152,10 +2146,7 @@ void RobotisController::setCtrlModuleThread(std::string ctrl_module) continue; uint32_t pos_data; - if(is_offset_enabled_) - pos_data = dxl->convertRadian2Value(dxl->dxl_state_->goal_position_ + dxl->dxl_state_->position_offset_); - else - pos_data = dxl->convertRadian2Value(dxl->dxl_state_->goal_position_); + pos_data = dxl->convertRadian2Value(dxl->dxl_state_->goal_position_ + dxl->dxl_state_->position_offset_ * offset_ratio_); uint8_t sync_write_data[4] = { 0 }; sync_write_data[0] = DXL_LOBYTE(DXL_LOWORD(pos_data)); @@ -2195,10 +2186,7 @@ void RobotisController::setCtrlModuleThread(std::string ctrl_module) if (mode == PositionControl) { uint32_t pos_data; - if(is_offset_enabled_) - pos_data = dxl->convertRadian2Value(dxl->dxl_state_->goal_position_ + dxl->dxl_state_->position_offset_); - else - pos_data = dxl->convertRadian2Value(dxl->dxl_state_->goal_position_); + pos_data = dxl->convertRadian2Value(dxl->dxl_state_->goal_position_ + dxl->dxl_state_->position_offset_ * offset_ratio_); uint8_t sync_write_data[4] = { 0 }; sync_write_data[0] = DXL_LOBYTE(DXL_LOWORD(pos_data));