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

Modif in write() fct

parent 214fa7e0
......@@ -528,7 +528,20 @@ SystemBoltHardware::write()
Eigen::Vector6d gain_KP;
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();
while (!robot_->IsTimeout()) {
......@@ -539,8 +552,8 @@ SystemBoltHardware::write()
if (robot_->IsReady()) {
// Control law
/*positions = ...;
torques = ...;*/
positions = ...;
torques = ...;
for (const hardware_interface::ComponentInfo & joint : info_.joints) {
positions[joint_name_to_motor_nb_[joint.name]] = hw_commands_[joint.name].position;
......@@ -584,7 +597,7 @@ SystemBoltHardware::write()
// }
// RCLCPP_INFO(
// rclcpp::get_logger("SystemBoltHardware"),
// "Joints sucessfully written!");
// "Joints sucessfully written!");*/
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