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

Modif init_robot and configure fonction

parent 457c2c2c
...@@ -136,10 +136,10 @@ public: ...@@ -136,10 +136,10 @@ public:
RCLCPP_SHARED_PTR_DEFINITIONS(SystemBoltHardware) RCLCPP_SHARED_PTR_DEFINITIONS(SystemBoltHardware)
ROS2_CONTROL_BOLT_PUBLIC ROS2_CONTROL_BOLT_PUBLIC
return_type configure(const hardware_interface::HardwareInfo & info) override; return_type configure();
ROS2_CONTROL_BOLT_PUBLIC ROS2_CONTROL_BOLT_PUBLIC
return_type init_robot(const hardware_interface::HardwareInfo & info); return_type init_robot();
ROS2_CONTROL_BOLT_PUBLIC ROS2_CONTROL_BOLT_PUBLIC
std::vector<hardware_interface::StateInterface> export_state_interfaces() override; std::vector<hardware_interface::StateInterface> export_state_interfaces() override;
......
...@@ -50,7 +50,7 @@ Eigen::Vector6d desired_joint_position = Eigen::Vector6d::Zero(); ...@@ -50,7 +50,7 @@ Eigen::Vector6d desired_joint_position = Eigen::Vector6d::Zero();
Eigen::Vector6d desired_torque = Eigen::Vector6d::Zero(); Eigen::Vector6d desired_torque = Eigen::Vector6d::Zero();
return_type SystemBoltHardware::init_robot(const hardware_interface::HardwareInfo & info) return_type SystemBoltHardware::init_robot()
{ {
//Define the ODRI robot from URDF values //Define the ODRI robot from URDF values
...@@ -61,7 +61,7 @@ return_type SystemBoltHardware::init_robot(const hardware_interface::HardwareInf ...@@ -61,7 +61,7 @@ return_type SystemBoltHardware::init_robot(const hardware_interface::HardwareInf
main_board_ptr_ = std::make_shared<MasterBoardInterface>(eth_interface_); 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"));
// Reversed polarities // Reversed polarities
...@@ -94,12 +94,12 @@ return_type SystemBoltHardware::init_robot(const hardware_interface::HardwareInf ...@@ -94,12 +94,12 @@ return_type SystemBoltHardware::init_robot(const hardware_interface::HardwareInf
safety_damping_); safety_damping_);
// Get position offset of each joint // Get position offset of each joint
for (const hardware_interface::ComponentInfo & joint : info.joints) { for (const hardware_interface::ComponentInfo & joint : info_.joints) {
position_offsets_[joint_name_to_motor_nb_[joint.name]] = stod(joint.parameters.at("position_offset")); //Modif d'après lecture des capteurs (demo bolt) position_offsets_[joint_name_to_motor_nb_[joint.name]] = stod(joint.parameters.at("position_offset")); //Modif d'après lecture des capteurs (demo bolt)
} }
// Define the IMU (ODRI). // Define the IMU (ODRI).
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++){
...@@ -121,10 +121,9 @@ return return_type::OK; ...@@ -121,10 +121,9 @@ return return_type::OK;
return_type SystemBoltHardware::configure( return_type SystemBoltHardware::configure()
const hardware_interface::HardwareInfo & info)
{ {
if (configure_default(info) != return_type::OK) { if (configure_default(info_) != return_type::OK) {
return return_type::ERROR; return return_type::ERROR;
} }
...@@ -436,7 +435,6 @@ return_type SystemBoltHardware::start() ...@@ -436,7 +435,6 @@ return_type SystemBoltHardware::start()
{ {
robot_->Start(); robot_->Start();
// set some default values // set some default values
for (const hardware_interface::ComponentInfo & joint : info_.joints) { for (const hardware_interface::ComponentInfo & joint : info_.joints) {
if (std::isnan(hw_states_[joint.name].position)) { if (std::isnan(hw_states_[joint.name].position)) {
...@@ -541,64 +539,6 @@ SystemBoltHardware::write() ...@@ -541,64 +539,6 @@ SystemBoltHardware::write()
robot_->joints->SetPositionGains(gain_KP); robot_->joints->SetPositionGains(gain_KP);
robot_->joints->SetVelocityGains(gain_KD); robot_->joints->SetVelocityGains(gain_KD);
/*
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
if (robot_->IsReady()) {
// Control law
positions = ...;
torques = ...;
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);
}
}
else
{
std::this_thread::yield();
}
robot_->SendCommand();
}
// RCLCPP_INFO(
// rclcpp::get_logger("SystemBoltHardware"),
// "Writing...");
// for (const hardware_interface::ComponentInfo & joint : info_.joints) {
// Simulate sending commands to the hardware
// RCLCPP_INFO(
// rclcpp::get_logger("SystemBoltHardware"),
// "Got command (%.5f,%.5f,%.5f,%.5f,%.5f) for joint %d!",
// hw_commands_[i].position,
// hw_commands_[i].velocity,
// hw_commands_[i].effort,
// hw_commands_[i].Kp,
// hw_commands_[i].Kd,
// i);
// }
// RCLCPP_INFO(
// rclcpp::get_logger("SystemBoltHardware"),
// "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