BRSRI/bellande_humanoid_robot_intelligence_control_system_framework
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Fixed body splashing when changing module between tuning_module and others.

Browse Source
This commit is contained in:
Kayman
2019-08-28 09:42:39 +09:00
parent 13457bd5dc
commit bd03350cc8
2 changed files with 34 additions and 45 deletions
Download Patch File Download Diff File Expand all files Collapse all files

3
robotis_controller/include/robotis_controller/robotis_controller.h
View File

@ -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_;

76
robotis_controller/src/robotis_controller/robotis_controller.cpp
View File

@ -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));