Commit 53a65068 authored by Paul Rouanet's avatar Paul Rouanet
Browse files

Config IMU in init_robot from URDF

parent 584fe11f
...@@ -96,21 +96,28 @@ return_type SystemBoltHardware::init_robot(const hardware_interface::HardwareInf ...@@ -96,21 +96,28 @@ return_type SystemBoltHardware::init_robot(const hardware_interface::HardwareInf
max_joint_velocities_, max_joint_velocities_,
safety_damping_); safety_damping_);
// Define the IMU (ODRI). // Define the IMU (ODRI).
rotate_vector_ << 1, 2, 3; for (const hardware_interface::ComponentInfo & sensor : info.sensors) {
orientation_vector_ << 1, 2, 3, 4; std::istringstream iss_rotate (sensor.parameters.at("rotate_vector"));
std::istringstream iss_orientation (sensor.parameters.at("orientation_vector"));
/*for (const hardware_interface::ComponentInfo & sensor : info.sensors) { for (int n=0; n<3; n++){
rotate_vector_ = stol(sensor.parameters.at("rotate_vector")); iss_rotate >> rotate_vector_[n];
//orientation_vector_ = sensor.parameters.at("orientation_vector"); }
}*/ for (int n=0; n<4; 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_);
// Define the robot (ODRI). // Define the robot (ODRI).
robot_ = std::make_shared<Robot>(main_board_ptr_, joints, imu); robot_ = std::make_shared<Robot>(main_board_ptr_, joints, imu);
// Initialize gains
kp_ = 0.0; kp_ = 0.0;
kd_ = 0.0; kd_ = 0.0;
......
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