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

About init_robot and IMU sensor for read function

parent 71f85861
......@@ -96,6 +96,15 @@ struct PosVelEffortGains
double Kd;
};
struct GyroAccLineEulerQuater
{
Eigen::Vector4l imu_gyro;
Eigen::Vector3l imu_accelero;
Eigen::Vector3l imu_line_acc;
Eigen::Vector3l imu_euler;
Eigen::Vector3l imu_quater;
};
constexpr const auto HW_IF_GAIN_KP = "gain_kp";
constexpr const auto HW_IF_GAIN_KD = "gain_kd";
......@@ -134,7 +143,7 @@ public:
return_type configure(const hardware_interface::HardwareInfo & info) override;
ROS2_CONTROL_BOLT_PUBLIC
return_type init_robot(const hardware_interface::HardwareInfo & info, auto& robot);
return_type init_robot(const hardware_interface::HardwareInfo & info, std::shared_ptr<odri_control_interface::Robot> & robot);
ROS2_CONTROL_BOLT_PUBLIC
std::vector<hardware_interface::StateInterface> export_state_interfaces() override;
......@@ -184,6 +193,10 @@ private:
std::map<std::string,PosVelEffortGains> hw_states_;
std::map<std::string,control_mode_t> control_mode_;
// Store the imu data
std::map<std::string,GyroAccLineEulerQuater> imu_states_;
std::map<std::string,control_mode_t> new_modes_;
//Definition of multiple variables about Bolt
......@@ -201,7 +214,7 @@ private:
char **argv;
//Joints constants
auto robot;
std::shared_ptr<odri_control_interface::Robot> robot;
double motor_constants = 0.025;
double gear_ratios = 9.;
......
......@@ -48,7 +48,7 @@ Eigen::Vector6d desired_joint_position = Eigen::Vector6d::Zero();
Eigen::Vector6d desired_torque = Eigen::Vector6d::Zero();
return_type SystemBoltHardware::init_robot(const hardware_interface::HardwareInfo & info, auto& robot)
return_type SystemBoltHardware::init_robot(const hardware_interface::HardwareInfo & info, std::shared_ptr<odri_control_interface::Robot> & robot)
{
/*Définir le robot (ODRI) à partir des variables de l'URDF*/
......@@ -149,6 +149,13 @@ return_type SystemBoltHardware::configure(
std::numeric_limits<double>::quiet_NaN(),
std::numeric_limits<double>::quiet_NaN()};
control_mode_[joint.name] = control_mode_t::NO_VALID_MODE;
imu_states_["IMU"] =
{std::numeric_limits<Eigen::Vector4l>::quiet_NaN(),
std::numeric_limits<Eigen::Vector3l>::quiet_NaN(),
std::numeric_limits<Eigen::Vector3l>::quiet_NaN(),
std::numeric_limits<Eigen::Vector3l>::quiet_NaN(),
std::numeric_limits<Eigen::Vector3l>::quiet_NaN()};
// SystemBolt has exactly 5 doubles for the state and
......@@ -498,11 +505,11 @@ hardware_interface::return_type SystemBoltHardware::read()
auto sensor_positions = robot->joints->GetPositions();
auto sensor_velocities = robot->joints->GetVelocities();
auto measured_torques = robot->joints->GetMeasuredTorques();
/* auto imu_gyro = imu->GetGyroscope();
auto imu_accelero = imu->GetAccelerometer();
auto imu_line_acc = imu->GetLinearAcceleration();
auto imu_euler = imu->GetAttitudeEuler();
auto imu_quater = imu->GetAttitudeQuaternion();*/
/*IMUSensor::orientation_ = robot->imu->GetGyroscope();
IMUSensor::angular_velocity_ = robot->imu->GetAccelerometer();
IMUSensor::linear_acceleration_ = robot->imu->GetLinearAcceleration();*/
//auto imu_euler = robot->imu->GetAttitudeEuler();
//auto imu_quater = robot->imu->GetAttitudeQuaternion();
// Assignment of sensor data to ros2_control variables
......@@ -510,11 +517,9 @@ hardware_interface::return_type SystemBoltHardware::read()
hw_states_[joint.name].position = sensor_positions[joint_name_to_motor_nb[joint.name]];
hw_states_[joint.name].velocity = sensor_velocities[joint_name_to_motor_nb[joint.name]];
hw_states_[joint.name].effort = measured_torques[joint_name_to_motor_nb[joint.name]];
//Printing test
std::cout << "Joints FLHAA: ";
hw_states_["FLHAA"].position;
std::cout << std::endl;
/*imu_states_["IMU"].orientation = imu_gyroscope;
imu_states_["IMU"].angular_velocity = imu_accelerometer;
imu_states_["IMU"].linear_acceleration = imu_linear_acceleration;*/
}
......
Supports Markdown
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