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

Merge pull request #20 from ROBOTIS-GIT/velocity_pid_gain

Browse Source 
- added velocity p/i/d gain and position i/d gain sync_write code.
This commit is contained in:
ROBOTIS-zerom
2016-08-12 16:48:09 +09:00
committed by GitHub
parent a415fff32f 0b21211127
commit 3ff7e4fc25
24 changed files with 319 additions and 7 deletions
Download Patch File Download Diff File Expand all files Collapse all files

5
robotis_controller/include/robotis_controller/robotis_controller.h
View File

@@ -109,6 +109,11 @@ public:
std::map<std::string, dynamixel::GroupSyncWrite *> port_to_sync_write_velocity_;
std::map<std::string, dynamixel::GroupSyncWrite *> port_to_sync_write_current_;
std::map<std::string, dynamixel::GroupSyncWrite *> port_to_sync_write_position_p_gain_;
std::map<std::string, dynamixel::GroupSyncWrite *> port_to_sync_write_position_i_gain_;
std::map<std::string, dynamixel::GroupSyncWrite *> port_to_sync_write_position_d_gain_;
std::map<std::string, dynamixel::GroupSyncWrite *> port_to_sync_write_velocity_p_gain_;
std::map<std::string, dynamixel::GroupSyncWrite *> port_to_sync_write_velocity_i_gain_;
std::map<std::string, dynamixel::GroupSyncWrite *> port_to_sync_write_velocity_d_gain_;
/* publisher */
ros::Publisher goal_joint_state_pub_;

242
robotis_controller/src/robotis_controller/robotis_controller.cpp
View File

@@ -93,11 +93,36 @@ void RobotisController::initializeSyncWrite()
if (it.second != NULL)
it.second->clearParam();
}
for (auto& it : port_to_sync_write_position_i_gain_)
{
if (it.second != NULL)
it.second->clearParam();
}
for (auto& it : port_to_sync_write_position_d_gain_)
{
if (it.second != NULL)
it.second->clearParam();
}
for (auto& it : port_to_sync_write_velocity_)
{
if (it.second != NULL)
it.second->clearParam();
}
for (auto& it : port_to_sync_write_velocity_p_gain_)
{
if (it.second != NULL)
it.second->clearParam();
}
for (auto& it : port_to_sync_write_velocity_i_gain_)
{
if (it.second != NULL)
it.second->clearParam();
}
for (auto& it : port_to_sync_write_velocity_d_gain_)
{
if (it.second != NULL)
it.second->clearParam();
}
for (auto& it : port_to_sync_write_current_)
{
if (it.second != NULL)
@@ -141,12 +166,37 @@ void RobotisController::initializeSyncWrite()
{
dxl->dxl_state_->position_p_gain_ = read_data;
}
else if ((dxl->position_i_gain_item_ != 0) &&
(dxl->bulk_read_items_[i]->item_name_ == dxl->position_i_gain_item_->item_name_))
{
dxl->dxl_state_->position_i_gain_ = read_data;
}
else if ((dxl->position_d_gain_item_ != 0) &&
(dxl->bulk_read_items_[i]->item_name_ == dxl->position_d_gain_item_->item_name_))
{
dxl->dxl_state_->position_d_gain_ = read_data;
}
else if ((dxl->present_velocity_item_ != 0) &&
(dxl->bulk_read_items_[i]->item_name_ == dxl->present_velocity_item_->item_name_))
{
dxl->dxl_state_->present_velocity_ = dxl->convertValue2Velocity(read_data);
dxl->dxl_state_->goal_velocity_ = dxl->dxl_state_->present_velocity_;
}
else if ((dxl->velocity_p_gain_item_ != 0) &&
(dxl->bulk_read_items_[i]->item_name_ == dxl->velocity_p_gain_item_->item_name_))
{
dxl->dxl_state_->velocity_p_gain_ = read_data;
}
else if ((dxl->velocity_i_gain_item_ != 0) &&
(dxl->bulk_read_items_[i]->item_name_ == dxl->velocity_i_gain_item_->item_name_))
{
dxl->dxl_state_->velocity_i_gain_ = read_data;
}
else if ((dxl->velocity_d_gain_item_ != 0) &&
(dxl->bulk_read_items_[i]->item_name_ == dxl->velocity_d_gain_item_->item_name_))
{
dxl->dxl_state_->velocity_d_gain_ = read_data;
}
else if ((dxl->present_current_item_ != 0) &&
(dxl->bulk_read_items_[i]->item_name_ == dxl->present_current_item_->item_name_))
{
@@ -210,6 +260,24 @@ bool RobotisController::initialize(const std::string robot_file_path, const std:
default_device->position_p_gain_item_->data_length_);
}
if (default_device->position_i_gain_item_ != 0)
{
port_to_sync_write_position_i_gain_[port_name]
= new dynamixel::GroupSyncWrite(port,
default_pkt_handler,
default_device->position_i_gain_item_->address_,
default_device->position_i_gain_item_->data_length_);
}
if (default_device->position_d_gain_item_ != 0)
{
port_to_sync_write_position_d_gain_[port_name]
= new dynamixel::GroupSyncWrite(port,
default_pkt_handler,
default_device->position_d_gain_item_->address_,
default_device->position_d_gain_item_->data_length_);
}
if (default_device->goal_velocity_item_ != 0)
{
port_to_sync_write_velocity_[port_name]
@@ -219,6 +287,33 @@ bool RobotisController::initialize(const std::string robot_file_path, const std:
default_device->goal_velocity_item_->data_length_);
}
if (default_device->velocity_p_gain_item_ != 0)
{
port_to_sync_write_velocity_p_gain_[port_name]
= new dynamixel::GroupSyncWrite(port,
default_pkt_handler,
default_device->velocity_p_gain_item_->address_,
default_device->velocity_p_gain_item_->data_length_);
}
if (default_device->velocity_i_gain_item_ != 0)
{
port_to_sync_write_velocity_i_gain_[port_name]
= new dynamixel::GroupSyncWrite(port,
default_pkt_handler,
default_device->velocity_i_gain_item_->address_,
default_device->velocity_i_gain_item_->data_length_);
}
if (default_device->velocity_d_gain_item_ != 0)
{
port_to_sync_write_velocity_d_gain_[port_name]
= new dynamixel::GroupSyncWrite(port,
default_pkt_handler,
default_device->velocity_d_gain_item_->address_,
default_device->velocity_d_gain_item_->data_length_);
}
if (default_device->goal_current_item_ != 0)
{
port_to_sync_write_current_[port_name]
@@ -687,11 +782,36 @@ void RobotisController::stopTimer()
if (it.second != NULL)
it.second->clearParam();
}
for (auto& it : port_to_sync_write_position_i_gain_)
{
if (it.second != NULL)
it.second->clearParam();
}
for (auto& it : port_to_sync_write_position_d_gain_)
{
if (it.second != NULL)
it.second->clearParam();
}
for (auto& it : port_to_sync_write_velocity_)
{
if (it.second != NULL)
it.second->clearParam();
}
for (auto& it : port_to_sync_write_velocity_p_gain_)
{
if (it.second != NULL)
it.second->clearParam();
}
for (auto& it : port_to_sync_write_velocity_i_gain_)
{
if (it.second != NULL)
it.second->clearParam();
}
for (auto& it : port_to_sync_write_velocity_d_gain_)
{
if (it.second != NULL)
it.second->clearParam();
}
for (auto& it : port_to_sync_write_current_)
{
if (it.second != NULL)
@@ -937,14 +1057,42 @@ void RobotisController::process()
if (dxl_state->position_p_gain_ != result_state->position_p_gain_)
{
dxl_state->position_p_gain_ = result_state->position_p_gain_;
uint8_t sync_write_p_gain[4] = { 0 };
sync_write_p_gain[0] = DXL_LOBYTE(DXL_LOWORD(dxl_state->position_p_gain_));
sync_write_p_gain[1] = DXL_HIBYTE(DXL_LOWORD(dxl_state->position_p_gain_));
sync_write_p_gain[2] = DXL_LOBYTE(DXL_HIWORD(dxl_state->position_p_gain_));
sync_write_p_gain[3] = DXL_HIBYTE(DXL_HIWORD(dxl_state->position_p_gain_));
uint8_t sync_write_data[4] = { 0 };
sync_write_data[0] = DXL_LOBYTE(DXL_LOWORD(dxl_state->position_p_gain_));
sync_write_data[1] = DXL_HIBYTE(DXL_LOWORD(dxl_state->position_p_gain_));
sync_write_data[2] = DXL_LOBYTE(DXL_HIWORD(dxl_state->position_p_gain_));
sync_write_data[3] = DXL_HIBYTE(DXL_HIWORD(dxl_state->position_p_gain_));
if (port_to_sync_write_position_p_gain_[dxl->port_name_] != NULL)
port_to_sync_write_position_p_gain_[dxl->port_name_]->addParam(dxl->id_, sync_write_p_gain);
port_to_sync_write_position_p_gain_[dxl->port_name_]->addParam(dxl->id_, sync_write_data);
}
// if position i gain value is changed -> sync write
if (dxl_state->position_i_gain_ != result_state->position_i_gain_)
{
dxl_state->position_i_gain_ = result_state->position_i_gain_;
uint8_t sync_write_data[4] = { 0 };
sync_write_data[0] = DXL_LOBYTE(DXL_LOWORD(dxl_state->position_i_gain_));
sync_write_data[1] = DXL_HIBYTE(DXL_LOWORD(dxl_state->position_i_gain_));
sync_write_data[2] = DXL_LOBYTE(DXL_HIWORD(dxl_state->position_i_gain_));
sync_write_data[3] = DXL_HIBYTE(DXL_HIWORD(dxl_state->position_i_gain_));
if (port_to_sync_write_position_i_gain_[dxl->port_name_] != NULL)
port_to_sync_write_position_i_gain_[dxl->port_name_]->addParam(dxl->id_, sync_write_data);
}
// if position d gain value is changed -> sync write
if (dxl_state->position_d_gain_ != result_state->position_d_gain_)
{
dxl_state->position_d_gain_ = result_state->position_d_gain_;
uint8_t sync_write_data[4] = { 0 };
sync_write_data[0] = DXL_LOBYTE(DXL_LOWORD(dxl_state->position_d_gain_));
sync_write_data[1] = DXL_HIBYTE(DXL_LOWORD(dxl_state->position_d_gain_));
sync_write_data[2] = DXL_LOBYTE(DXL_HIWORD(dxl_state->position_d_gain_));
sync_write_data[3] = DXL_HIBYTE(DXL_HIWORD(dxl_state->position_d_gain_));
if (port_to_sync_write_position_d_gain_[dxl->port_name_] != NULL)
port_to_sync_write_position_d_gain_[dxl->port_name_]->addParam(dxl->id_, sync_write_data);
}
}
}
@@ -963,6 +1111,48 @@ void RobotisController::process()
if (port_to_sync_write_velocity_[dxl->port_name_] != NULL)
port_to_sync_write_velocity_[dxl->port_name_]->changeParam(dxl->id_, sync_write_data);
// if velocity p gain gain value is changed -> sync write
if (dxl_state->velocity_p_gain_ != result_state->velocity_p_gain_)
{
dxl_state->velocity_p_gain_ = result_state->velocity_p_gain_;
uint8_t sync_write_data[4] = { 0 };
sync_write_data[0] = DXL_LOBYTE(DXL_LOWORD(dxl_state->velocity_p_gain_));
sync_write_data[1] = DXL_HIBYTE(DXL_LOWORD(dxl_state->velocity_p_gain_));
sync_write_data[2] = DXL_LOBYTE(DXL_HIWORD(dxl_state->velocity_p_gain_));
sync_write_data[3] = DXL_HIBYTE(DXL_HIWORD(dxl_state->velocity_p_gain_));
if (port_to_sync_write_velocity_p_gain_[dxl->port_name_] != NULL)
port_to_sync_write_velocity_p_gain_[dxl->port_name_]->addParam(dxl->id_, sync_write_data);
}
// if velocity i gain value is changed -> sync write
if (dxl_state->velocity_i_gain_ != result_state->velocity_i_gain_)
{
dxl_state->velocity_i_gain_ = result_state->velocity_i_gain_;
uint8_t sync_write_data[4] = { 0 };
sync_write_data[0] = DXL_LOBYTE(DXL_LOWORD(dxl_state->velocity_i_gain_));
sync_write_data[1] = DXL_HIBYTE(DXL_LOWORD(dxl_state->velocity_i_gain_));
sync_write_data[2] = DXL_LOBYTE(DXL_HIWORD(dxl_state->velocity_i_gain_));
sync_write_data[3] = DXL_HIBYTE(DXL_HIWORD(dxl_state->velocity_i_gain_));
if (port_to_sync_write_velocity_i_gain_[dxl->port_name_] != NULL)
port_to_sync_write_velocity_i_gain_[dxl->port_name_]->addParam(dxl->id_, sync_write_data);
}
// if velocity d gain value is changed -> sync write
if (dxl_state->velocity_d_gain_ != result_state->velocity_d_gain_)
{
dxl_state->velocity_d_gain_ = result_state->velocity_d_gain_;
uint8_t sync_write_data[4] = { 0 };
sync_write_data[0] = DXL_LOBYTE(DXL_LOWORD(dxl_state->velocity_d_gain_));
sync_write_data[1] = DXL_HIBYTE(DXL_LOWORD(dxl_state->velocity_d_gain_));
sync_write_data[2] = DXL_LOBYTE(DXL_HIWORD(dxl_state->velocity_d_gain_));
sync_write_data[3] = DXL_HIBYTE(DXL_HIWORD(dxl_state->velocity_d_gain_));
if (port_to_sync_write_velocity_d_gain_[dxl->port_name_] != NULL)
port_to_sync_write_velocity_d_gain_[dxl->port_name_]->addParam(dxl->id_, sync_write_data);
}
}
}
else if ((*module_it)->getControlMode() == TorqueControl)
@@ -1004,6 +1194,46 @@ void RobotisController::process()
it.second->clearParam();
}
}
if (port_to_sync_write_position_i_gain_.size() > 0)
{
for (auto& it : port_to_sync_write_position_i_gain_)
{
it.second->txPacket();
it.second->clearParam();
}
}
if (port_to_sync_write_position_d_gain_.size() > 0)
{
for (auto& it : port_to_sync_write_position_d_gain_)
{
it.second->txPacket();
it.second->clearParam();
}
}
if (port_to_sync_write_velocity_p_gain_.size() > 0)
{
for (auto& it : port_to_sync_write_velocity_p_gain_)
{
it.second->txPacket();
it.second->clearParam();
}
}
if (port_to_sync_write_velocity_i_gain_.size() > 0)
{
for (auto& it : port_to_sync_write_velocity_i_gain_)
{
it.second->txPacket();
it.second->clearParam();
}
}
if (port_to_sync_write_velocity_d_gain_.size() > 0)
{
for (auto& it : port_to_sync_write_velocity_d_gain_)
{
it.second->txPacket();
it.second->clearParam();
}
}
for (auto& it : port_to_sync_write_position_)
{
if (it.second != NULL)

3
robotis_device/devices/dynamixel/GRIPPER.device
View File

@@ -19,6 +19,9 @@ goal_current_item_name = goal_torque
position_d_gain_item_name =
position_i_gain_item_name =
position_p_gain_item_name = position_p_gain
velocity_d_gain_item_name =
velocity_i_gain_item_name = velocity_i_gain
velocity_p_gain_item_name = velocity_p_gain
[control table]
# addr | item name | length | access | memory | min value | max value | signed

3
robotis_device/devices/dynamixel/H42-20-S300-R.device
View File

@@ -21,6 +21,9 @@ goal_current_item_name = goal_torque
position_d_gain_item_name =
position_i_gain_item_name =
position_p_gain_item_name = position_p_gain
velocity_d_gain_item_name =
velocity_i_gain_item_name = velocity_i_gain
velocity_p_gain_item_name = velocity_p_gain
[control table]
# addr | item name | length | access | memory | min value | max value | signed

3
robotis_device/devices/dynamixel/H54-100-S500-R.device
View File

@@ -21,6 +21,9 @@ goal_current_item_name = goal_torque
position_d_gain_item_name =
position_i_gain_item_name =
position_p_gain_item_name = position_p_gain
velocity_d_gain_item_name =
velocity_i_gain_item_name = velocity_i_gain
velocity_p_gain_item_name = velocity_p_gain
[control table]
# addr | item name | length | access | memory | min value | max value | signed

3
robotis_device/devices/dynamixel/H54-200-B500-R.device
View File

@@ -21,6 +21,9 @@ goal_current_item_name = goal_torque
position_d_gain_item_name =
position_i_gain_item_name =
position_p_gain_item_name = position_p_gain
velocity_d_gain_item_name =
velocity_i_gain_item_name = velocity_i_gain
velocity_p_gain_item_name = velocity_p_gain
[control table]
# addr | item name | length | access | memory | min value | max value | signed

3
robotis_device/devices/dynamixel/H54-200-S500-R.device
View File

@@ -21,6 +21,9 @@ goal_current_item_name = goal_torque
position_d_gain_item_name =
position_i_gain_item_name =
position_p_gain_item_name = position_p_gain
velocity_d_gain_item_name =
velocity_i_gain_item_name = velocity_i_gain
velocity_p_gain_item_name = velocity_p_gain
[control table]
# addr | item name | length | access | memory | min value | max value | signed

3
robotis_device/devices/dynamixel/L42-10-S300-R.device
View File

@@ -19,6 +19,9 @@ goal_current_item_name = goal_torque
position_d_gain_item_name =
position_i_gain_item_name =
position_p_gain_item_name = position_p_gain
velocity_d_gain_item_name =
velocity_i_gain_item_name = velocity_i_gain
velocity_p_gain_item_name = velocity_p_gain
[control table]
# addr | item name | length | access | memory | min value | max value | signed

3
robotis_device/devices/dynamixel/L54-30-S400-R.device
View File

@@ -19,6 +19,9 @@ goal_current_item_name = goal_torque
position_d_gain_item_name =
position_i_gain_item_name =
position_p_gain_item_name = position_p_gain
velocity_d_gain_item_name =
velocity_i_gain_item_name = velocity_i_gain
velocity_p_gain_item_name = velocity_p_gain
[control table]
# addr | item name | length | access | memory | min value | max value | signed

3
robotis_device/devices/dynamixel/L54-30-S500-R.device
View File

@@ -19,6 +19,9 @@ goal_current_item_name = goal_torque
position_d_gain_item_name =
position_i_gain_item_name =
position_p_gain_item_name = position_p_gain
velocity_d_gain_item_name =
velocity_i_gain_item_name = velocity_i_gain
velocity_p_gain_item_name = velocity_p_gain
[control table]
# addr | item name | length | access | memory | min value | max value | signed

3
robotis_device/devices/dynamixel/L54-50-S290-R.device
View File

@@ -19,6 +19,9 @@ goal_current_item_name = goal_torque
position_d_gain_item_name =
position_i_gain_item_name =
position_p_gain_item_name = position_p_gain
velocity_d_gain_item_name =
velocity_i_gain_item_name = velocity_i_gain
velocity_p_gain_item_name = velocity_p_gain
[control table]
# addr | item name | length | access | memory | min value | max value | signed

3
robotis_device/devices/dynamixel/L54-50-S500-R.device
View File

@@ -19,6 +19,9 @@ goal_current_item_name = goal_torque
position_d_gain_item_name =
position_i_gain_item_name =
position_p_gain_item_name = position_p_gain
velocity_d_gain_item_name =
velocity_i_gain_item_name = velocity_i_gain
velocity_p_gain_item_name = velocity_p_gain
[control table]
# addr | item name | length | access | memory | min value | max value | signed

3
robotis_device/devices/dynamixel/M42-10-S260-R.device
View File

@@ -19,6 +19,9 @@ goal_current_item_name = goal_torque
position_d_gain_item_name =
position_i_gain_item_name =
position_p_gain_item_name = position_p_gain
velocity_d_gain_item_name =
velocity_i_gain_item_name = velocity_i_gain
velocity_p_gain_item_name = velocity_p_gain
[control table]
# addr | item name | length | access | memory | min value | max value | signed

3
robotis_device/devices/dynamixel/M54-40-S250-R.device
View File

@@ -19,6 +19,9 @@ goal_current_item_name = goal_torque
position_d_gain_item_name =
position_i_gain_item_name =
position_p_gain_item_name = position_p_gain
velocity_d_gain_item_name =
velocity_i_gain_item_name = velocity_i_gain
velocity_p_gain_item_name = velocity_p_gain
[control table]
# addr | item name | length | access | memory | min value | max value | signed

3
robotis_device/devices/dynamixel/M54-60-S250-R.device
View File

@@ -19,6 +19,9 @@ goal_current_item_name = goal_torque
position_d_gain_item_name =
position_i_gain_item_name =
position_p_gain_item_name = position_p_gain
velocity_d_gain_item_name =
velocity_i_gain_item_name = velocity_i_gain
velocity_p_gain_item_name = velocity_p_gain
[control table]
# addr | item name | length | access | memory | min value | max value | signed

3
robotis_device/devices/dynamixel/MX-106.device
View File

@@ -19,6 +19,9 @@ goal_current_item_name =
position_d_gain_item_name = position_d_gain
position_i_gain_item_name = position_i_gain
position_p_gain_item_name = position_p_gain
velocity_d_gain_item_name =
velocity_i_gain_item_name =
velocity_p_gain_item_name =
[control table]
# addr | item name | length | access | memory | min value | max value | signed

3
robotis_device/devices/dynamixel/MX-28.device
View File

@@ -19,6 +19,9 @@ goal_current_item_name =
position_d_gain_item_name = position_d_gain
position_i_gain_item_name = position_i_gain
position_p_gain_item_name = position_p_gain
velocity_d_gain_item_name =
velocity_i_gain_item_name =
velocity_p_gain_item_name =
[control table]
# addr | item name | length | access | memory | min value | max value | signed

3
robotis_device/devices/dynamixel/MX-64.device
View File

@@ -19,6 +19,9 @@ goal_current_item_name =
position_d_gain_item_name = position_d_gain
position_i_gain_item_name = position_i_gain
position_p_gain_item_name = position_p_gain
velocity_d_gain_item_name =
velocity_i_gain_item_name =
velocity_p_gain_item_name =
[control table]
# addr | item name | length | access | memory | min value | max value | signed

3
robotis_device/devices/dynamixel/XM-430-W210.device
View File

@@ -22,6 +22,9 @@ goal_current_item_name = goal_current
position_d_gain_item_name = position_d_gain
position_i_gain_item_name = position_i_gain
position_p_gain_item_name = position_p_gain
velocity_d_gain_item_name =
velocity_i_gain_item_name = velocity_i_gain
velocity_p_gain_item_name = velocity_p_gain
[control table]
# addr | item name | length | access | memory | min value | max value | signed

3
robotis_device/devices/dynamixel/XM-430-W350.device
View File

@@ -22,6 +22,9 @@ goal_current_item_name = goal_current
position_d_gain_item_name = position_d_gain
position_i_gain_item_name = position_i_gain
position_p_gain_item_name = position_p_gain
velocity_d_gain_item_name =
velocity_i_gain_item_name = velocity_i_gain
velocity_p_gain_item_name = velocity_p_gain
[control table]
# addr | item name | length | access | memory | min value | max value | signed

3
robotis_device/devices/dynamixel/XM-430.device
View File

@@ -21,6 +21,9 @@ goal_current_item_name = goal_current
position_d_gain_item_name = position_d_gain
position_i_gain_item_name = position_i_gain
position_p_gain_item_name = position_p_gain
velocity_d_gain_item_name =
velocity_i_gain_item_name = velocity_i_gain
velocity_p_gain_item_name = velocity_p_gain
[control table]
# addr | item name | length | access | memory | min value | max value | signed

5
robotis_device/include/robotis_device/dynamixel.h
View File

@@ -73,6 +73,11 @@ public:
ControlTableItem *goal_velocity_item_;
ControlTableItem *goal_current_item_;
ControlTableItem *position_p_gain_item_;
ControlTableItem *position_i_gain_item_;
ControlTableItem *position_d_gain_item_;
ControlTableItem *velocity_p_gain_item_;
ControlTableItem *velocity_i_gain_item_;
ControlTableItem *velocity_d_gain_item_;
Dynamixel(int id, std::string model_name, float protocol_version);

10
robotis_device/include/robotis_device/dynamixel_state.h
View File

@@ -60,6 +60,11 @@ public:
double goal_velocity_;
double goal_torque_;
double position_p_gain_;
double position_i_gain_;
double position_d_gain_;
double velocity_p_gain_;
double velocity_i_gain_;
double velocity_d_gain_;
std::map<std::string, uint32_t> bulk_read_table_;
@@ -74,6 +79,11 @@ public:
goal_velocity_(0.0),
goal_torque_(0.0),
position_p_gain_(0.0),
position_i_gain_(0.0),
position_d_gain_(0.0),
velocity_p_gain_(0.0),
velocity_i_gain_(0.0),
velocity_d_gain_(0.0),
position_offset_(0.0)
{
bulk_read_table_.clear();

7
robotis_device/src/robotis_device/dynamixel.cpp
View File

@@ -55,7 +55,12 @@ Dynamixel::Dynamixel(int id, std::string model_name, float protocol_version)
goal_position_item_(0),
goal_velocity_item_(0),
goal_current_item_(0),
position_p_gain_item_(0)
position_p_gain_item_(0),
position_i_gain_item_(0),
position_d_gain_item_(0),
velocity_p_gain_item_(0),
velocity_i_gain_item_(0),
velocity_d_gain_item_(0)
{
this->id_ = id;
this->model_name_ = model_name;