remove unused correct z function

userFiles/interface/io/controller_inputs.cc

@@ -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
 }