Commit 81d6c897 authored by Nicolas Van der Noot's avatar Nicolas Van der Noot

remove unused correct z function

parent e920fe52
......@@ -17,9 +17,6 @@
#include "get_simu_id_interface.hh"
#include "user_IO.h"
// function prototype
void correct_unknow_z_IMU(Inputs_ctrl *ivs);
/*! \brief assigns the inputs of the controller
*
* \param[in] mbs_data Robotran structure
......@@ -179,72 +176,4 @@ void controller_inputs(MbsData *mbs_data, MotorCoManIndex *coman_index, Inputs_c
ivs->IMU_Angular_Rate[i] = sens_info->SMidWaistOM(i); // angulare rate -> velocity [rad/s]
ivs->IMU_Acceleration[i] = sens_info->SMidWaistOMP(i); // acceleration [rad/s^2]
}
// info not accurately available on the real COMAN
//correct_unknow_z_IMU(ivs);
}
/*! \brief correct the IMU information for unknown z axis
*
* \param[in,out] ivs inputs structure
*/
void correct_unknow_z_IMU(Inputs_ctrl *ivs)
{
double IMU11, IMU12, IMU13, IMU23, IMU33;
double theta_1, theta_2, theta_3;
double theta_p_1, theta_p_2, theta_p_3;
double omega_1, omega_2, omega_3;
double inv_c2;
double c1, c2, c3;
double s1, s2, s3;
// rotation matrix transformed for unknown z axis
IMU11 = ivs->IMU_Orientation[0];
IMU12 = ivs->IMU_Orientation[1];
IMU13 = ivs->IMU_Orientation[2];
IMU23 = ivs->IMU_Orientation[5];
IMU33 = ivs->IMU_Orientation[8];
theta_1 = atan2(IMU23, IMU33);
theta_2 = atan2(-IMU13, sqrt(IMU11*IMU11 + IMU12*IMU12));
theta_3 = 0.0; // atan2(IMU12, IMU11) to get correct angle
c1 = cos(theta_1);
c2 = cos(theta_2);
c3 = cos(theta_3);
s1 = sin(theta_1);
s2 = sin(theta_2);
s3 = sin(theta_3);
ivs->IMU_Orientation[0] = c2*c3;
ivs->IMU_Orientation[1] = c2*s3;
ivs->IMU_Orientation[2] = -s2;
ivs->IMU_Orientation[3] = -c1*s3 + s1*s2*c3;
ivs->IMU_Orientation[4] = c1*c3 + s1*s2*s3;
ivs->IMU_Orientation[5] = s1*c2;
ivs->IMU_Orientation[6] = s1*s3 + c1*s2*c3;
ivs->IMU_Orientation[7] = -s1*c3 + c1*s2*s3;
ivs->IMU_Orientation[8] = c1*c2;
if (!c2)
{
return;
}
omega_1 = ivs->IMU_Angular_Rate[0];
omega_2 = ivs->IMU_Angular_Rate[1];
omega_3 = ivs->IMU_Angular_Rate[2];
inv_c2 = 1.0 / c2;
theta_p_1 = inv_c2 * (c3*omega_1 + s3*omega_2);
theta_p_2 = c3*omega_2 - s3*omega_1;
theta_p_3 = 0.0; // inv_c2 * s2 * (s3*omega_2 + c3*omega_1) + omega_3 to get correct angle derivative
ivs->IMU_Angular_Rate[0] = c2*c3*theta_p_1 - s3*theta_p_2;
ivs->IMU_Angular_Rate[1] = c2*s3*theta_p_1 + c3*theta_p_2;
ivs->IMU_Angular_Rate[2] = -s2*theta_p_1 + theta_p_3;
// ivs->IMU_Acceleration not corrected
}
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