Commit b2b04c4b authored by Paul Rouanet's avatar Paul Rouanet
Browse files

Clean the code (delete commented code in init_robot)

parent 53a65068
...@@ -52,18 +52,15 @@ Eigen::Vector6d desired_torque = Eigen::Vector6d::Zero(); ...@@ -52,18 +52,15 @@ Eigen::Vector6d desired_torque = Eigen::Vector6d::Zero();
return_type SystemBoltHardware::init_robot(const hardware_interface::HardwareInfo & info) return_type SystemBoltHardware::init_robot(const hardware_interface::HardwareInfo & info)
{ {
//Define the ODRI robot from URDF values
// Get the ethernet interface to discuss with the ODRI master board // Get the ethernet interface to discuss with the ODRI master board
eth_interface_ = info_.hardware_parameters.at("eth_interface"); eth_interface_ = info_.hardware_parameters.at("eth_interface");
/*Définir le robot (ODRI) à partir des variables de l'URDF*/
// Define board (ODRI) // Define board (ODRI)
auto main_board_ptr_ = std::make_shared<MasterBoardInterface>(eth_interface_); auto main_board_ptr_ = std::make_shared<MasterBoardInterface>(eth_interface_);
// Define joints (ODRI) // Define joints (ODRI)
for (const hardware_interface::ComponentInfo & joint : info.joints) { for (const hardware_interface::ComponentInfo & joint : info.joints) {
// Motor numbers // Motor numbers
motor_numbers_[joint_name_to_motor_nb_[joint.name]] = stoi(joint.parameters.at("motor_number")); motor_numbers_[joint_name_to_motor_nb_[joint.name]] = stoi(joint.parameters.at("motor_number"));
...@@ -101,7 +98,6 @@ return_type SystemBoltHardware::init_robot(const hardware_interface::HardwareInf ...@@ -101,7 +98,6 @@ return_type SystemBoltHardware::init_robot(const hardware_interface::HardwareInf
for (const hardware_interface::ComponentInfo & sensor : info.sensors) { for (const hardware_interface::ComponentInfo & sensor : info.sensors) {
std::istringstream iss_rotate (sensor.parameters.at("rotate_vector")); std::istringstream iss_rotate (sensor.parameters.at("rotate_vector"));
std::istringstream iss_orientation (sensor.parameters.at("orientation_vector")); std::istringstream iss_orientation (sensor.parameters.at("orientation_vector"));
for (int n=0; n<3; n++){ for (int n=0; n<3; n++){
iss_rotate >> rotate_vector_[n]; iss_rotate >> rotate_vector_[n];
} }
...@@ -109,7 +105,6 @@ return_type SystemBoltHardware::init_robot(const hardware_interface::HardwareInf ...@@ -109,7 +105,6 @@ return_type SystemBoltHardware::init_robot(const hardware_interface::HardwareInf
iss_orientation >> orientation_vector_[n]; iss_orientation >> orientation_vector_[n];
} }
} }
auto imu = std::make_shared<IMU>( auto imu = std::make_shared<IMU>(
main_board_ptr_, rotate_vector_, orientation_vector_); main_board_ptr_, rotate_vector_, orientation_vector_);
...@@ -121,33 +116,11 @@ return_type SystemBoltHardware::init_robot(const hardware_interface::HardwareInf ...@@ -121,33 +116,11 @@ return_type SystemBoltHardware::init_robot(const hardware_interface::HardwareInf
kp_ = 0.0; kp_ = 0.0;
kd_ = 0.0; kd_ = 0.0;
/*
for (const hardware_interface::ComponentInfo & joint : info.joints) {
//Données des joints
//Sorties (commandes)
joint.command_interfaces;
//Entrées (états)
joint.state_interfaces;
}
for (const hardware_interface::ComponentInfo & sensor : info.sensors) {
//Données des capteurs
//Entrées (états)
sensor.state_interfaces;
}
*/
return return_type::OK; return return_type::OK;
} }
return_type SystemBoltHardware::configure( return_type SystemBoltHardware::configure(
const hardware_interface::HardwareInfo & info) const hardware_interface::HardwareInfo & info)
{ {
......
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