Commit 24caf540 authored by Paul Rouanet's avatar Paul Rouanet
Browse files

Modif stop function

parent 972e7820
...@@ -113,16 +113,6 @@ return_type SystemBoltHardware::init_robot(const hardware_interface::HardwareInf ...@@ -113,16 +113,6 @@ return_type SystemBoltHardware::init_robot(const hardware_interface::HardwareInf
kp_ = 0.0; kp_ = 0.0;
kd_ = 0.0; kd_ = 0.0;
//Definition of Kp and Kd :
/*std::cout << "Enter Kp value (3 in casual use) : ";
std::cin >> kp_;
std::cout << "Kp = " << kp_;
std::cout << "\n" << std::endl;
std::cout << "Enter Kd value (0.05 in casual use) : ";
std::cin >> kd_;
std::cout << "Kd = " << kd_;
std::cout << "\n" << std::endl;*/
/* /*
for (const hardware_interface::ComponentInfo & joint : info.joints) { for (const hardware_interface::ComponentInfo & joint : info.joints) {
...@@ -513,8 +503,32 @@ return_type SystemBoltHardware::start() ...@@ -513,8 +503,32 @@ return_type SystemBoltHardware::start()
return_type SystemBoltHardware::stop() return_type SystemBoltHardware::stop()
{ {
// Set 0 values to commands // Set 0 values to commands
for (const hardware_interface::ComponentInfo & joint : info_.joints) { /*for (const hardware_interface::ComponentInfo & joint : info_.joints) {
hw_commands_[joint.name] = {0.0, 0.0, 0.0, 0.0, 0.0}; hw_commands_[joint.name] = {0.0, 0.0, 0.0, 0.0, 0.0};
}*/
// Put torques to 0.0
Eigen::Vector6d torques;
double t = 0.0;
double dt = 0.001;
std::chrono::time_point<std::chrono::system_clock> last = std::chrono::system_clock::now();
while (!robot_->IsTimeout()) {
if (((std::chrono::duration<double>)(std::chrono::system_clock::now()-last)).count() > 0.001) {
last = std::chrono::system_clock::now(); // last+dt would be better
t += dt;
for (const hardware_interface::ComponentInfo & joint : info_.joints) {
torques[joint_name_to_motor_nb_[joint.name]] = 0.0;
}
robot_->joints->SetTorques(torques);
}
robot_->SendCommand();
} }
......
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