Commit 457c2c2c authored by Paul Rouanet's avatar Paul Rouanet
Browse files

Modif in write() fct

parent 214fa7e0
...@@ -528,7 +528,20 @@ SystemBoltHardware::write() ...@@ -528,7 +528,20 @@ SystemBoltHardware::write()
Eigen::Vector6d gain_KP; Eigen::Vector6d gain_KP;
Eigen::Vector6d gain_KD; Eigen::Vector6d gain_KD;
for (const hardware_interface::ComponentInfo & joint : info_.joints) {
positions[joint_name_to_motor_nb_[joint.name]] = hw_commands_[joint.name].position;
velocities[joint_name_to_motor_nb_[joint.name]] = hw_commands_[joint.name].velocity;
torques[joint_name_to_motor_nb_[joint.name]] = hw_commands_[joint.name].effort;
gain_KP[joint_name_to_motor_nb_[joint.name]] = hw_commands_[joint.name].Kp;
gain_KD[joint_name_to_motor_nb_[joint.name]] = hw_commands_[joint.name].Kd;
}
robot_->joints->SetDesiredPositions(positions);
robot_->joints->SetDesiredVelocities(velocities);
robot_->joints->SetTorques(torques);
robot_->joints->SetPositionGains(gain_KP);
robot_->joints->SetVelocityGains(gain_KD);
/*
std::chrono::time_point<std::chrono::system_clock> last = std::chrono::system_clock::now(); std::chrono::time_point<std::chrono::system_clock> last = std::chrono::system_clock::now();
while (!robot_->IsTimeout()) { while (!robot_->IsTimeout()) {
...@@ -539,8 +552,8 @@ SystemBoltHardware::write() ...@@ -539,8 +552,8 @@ SystemBoltHardware::write()
if (robot_->IsReady()) { if (robot_->IsReady()) {
// Control law // Control law
/*positions = ...; positions = ...;
torques = ...;*/ torques = ...;
for (const hardware_interface::ComponentInfo & joint : info_.joints) { for (const hardware_interface::ComponentInfo & joint : info_.joints) {
positions[joint_name_to_motor_nb_[joint.name]] = hw_commands_[joint.name].position; positions[joint_name_to_motor_nb_[joint.name]] = hw_commands_[joint.name].position;
...@@ -584,7 +597,7 @@ SystemBoltHardware::write() ...@@ -584,7 +597,7 @@ SystemBoltHardware::write()
// } // }
// RCLCPP_INFO( // RCLCPP_INFO(
// rclcpp::get_logger("SystemBoltHardware"), // rclcpp::get_logger("SystemBoltHardware"),
// "Joints sucessfully written!"); // "Joints sucessfully written!");*/
return return_type::OK; return return_type::OK;
} }
......
Markdown is supported
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment