//---------------------------------------------------------------------------
const Rvector3& SpiceAttitude::GetEulerAngleRates(Real atTime)
{
ComputeCosineMatrixAndAngularVelocity(atTime);
attitudeTime = atTime;
eulerAngles = GetEulerAngles(atTime);
eulerAngleRates = Attitude::ToEulerAngleRates(angVel,
eulerAngles,
(Integer) eulerSequenceArray.at(0),
(Integer) eulerSequenceArray.at(1),
(Integer) eulerSequenceArray.at(2));
return eulerAngleRates;
}
void GetEEPosition(CLink *Skelet, MDFloat *PnIter, MDFloat *OnIter) {
/* **************************************************************************************************************************** */
MDFloat (*ZTN)[4];
/* **************************************************************************************************************************** */
ZTN = (MDFloat(*)[4])Skelet[_NO_OF_LINKS_-1].Get_ZTi();
/* **************************************************************************************************************************** */
PnIter[0] = ZTN[0][3];
PnIter[1] = ZTN[1][3];
PnIter[2] = ZTN[2][3];
PnIter[3] = 1.0;
/* **************************************************************************************************************************** */
GetEulerAngles(ZTN, OnIter);
OnIter[3] = 1.0;
/* **************************************************************************************************************************** */
}
void read_yaw(const sensor_msgs::Imu::ConstPtr& msgyaw)
{
msgyaw_in.orientation.x = msgyaw->orientation.x;
msgyaw_in.orientation.y = msgyaw->orientation.y;
msgyaw_in.orientation.z = msgyaw->orientation.z;
msgyaw_in.orientation.w = msgyaw->orientation.w;
Quaternionm myq;
myq.x = msgyaw->orientation.x;
myq.y = msgyaw->orientation.y;
myq.z = msgyaw->orientation.z;
myq.w = msgyaw->orientation.w;
GetEulerAngles(myq, yaw, pitch, roll);
//ROS_INFO("Angles %lf %lf %lf",yaw,pitch,roll);
}
void InnerIterate() {
/* **************************************************************************************************************************** */
int i;
int NoOfIter;
MDFloat dX[_JAC_N_]; // Vecteur d'entrée (position & orientation désirées)
MDFloat dTheta[_JAC_M_]; // Vecteur de sortie (DDL : oX1,oY1,oZ1,oX2,oY2,oZ2,etc.)
MDFloat (*ZTN)[4];
MDFloat TempJt[_JAC_N_][_JAC_M_];
MDFloat JdThetaP[4]; // Vecteurs d'erreur
MDFloat JdThetaO[4];
MDFloat ErrVectP[4];
MDFloat ErrVectO[4];
MDFloat Pn[4]; // Position actuelle de l'effecteur
MDFloat dXPn[4]; // Variation désirée en position : dXPn = PNew - Pn
MDFloat On[4]; // Orientation actuelle de l'effecteur
MDFloat dXOn[4]; // Variation désirée en orientation : dXOn = ONew - On
MDFloat Err; // Erreur
CLink BackUpSkelet[_NO_OF_LINKS_];
/* **************************************************************************************************************************** */
NoOfIter = 0;
// Sauvegarde du squelette **
memcpy(BackUpSkelet, Skelet, _NO_OF_LINKS_*sizeof(CLink));
/* **************************************************************************************************************************** */
/////////////////////////////
// Calcul du vecteur d'entrée
/////////////////////////////
// Intilisation du vecteur d'entrée
// ..
for(i = 0; i<_JAC_N_;i++)dX[i] = 0.0;
// Calcul de la variation de position "dXPn" du vecteur d'entrée "dX"
// ..
ZTN = (MDFloat(*)[4])Skelet[_NO_OF_LINKS_-1].Get_ZTi();
Pn[0] = ZTN[0][3];
Pn[1] = ZTN[1][3];
Pn[2] = ZTN[2][3];
Pn[3] = 1.0;
VectSub(PNew,Pn,dXPn);
// Calcul de la variation en orientation "dXOn" du vecteur d'entrée "dX"
// ..
GetEulerAngles(ZTN, On);
On[3] = 1.0;
VectSub(ONew, On, dXOn);
dX[0] = dXPn[0]*0.5;
dX[1] = dXPn[1]*0.5;
dX[2] = dXPn[2]*0.5;
dX[3] = dXOn[0]*0.5;
dX[4] = dXOn[1]*0.5;
dX[5] = dXOn[2]*0.5;
/* **************************************************************************************************************************** */
///////////////////////////////////////
// Calcul du vecteur de sortie "dTheta"
///////////////////////////////////////
do { // Améliore la convergence de l'algorithme **
memcpy(Skelet, BackUpSkelet, _NO_OF_LINKS_*sizeof(CLink));
/* ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ */
// Calcul de la Jacobienne
// ..
MakeJacobian(Skelet,J);
/* ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ */
// Calcul de la Jacobienne transposée
// ..
TransposeJacobian(J, Jt);
/* ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ */
// Incorporation d'une raideur à la Jacobienne : poids forts aux articulations éloignée de l'effecteur.
// ..
memcpy(TempJt, Jt, _JAC_N_*_JAC_M_*sizeof(MDFloat));
MultKJacobianT(K , TempJt , Jt);
/* ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ */
// Calcul du vecteur de sortie : dTheta = Jt * dX
// ..
MultJacobianT(Jt, dX, dTheta );
/* ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ */
// Ajout de dTheta aux angles et mise-à-jour des matrices de transformation locales "m1Ti" du squelette
// ..
for (i=0; i<_NO_OF_LINKS_; i++)
Skelet[i].Updateim1Ti(dTheta[i*3+0], dTheta[i*3+1], dTheta[i*3+2]);
/* ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ */
// Estimation de l'erreur : Err² = (ErrVectP)² + (ErrVectO)²
// ..
UpdateZTiMatrices(Skelet);
GetEEPosition(Skelet, PnIter, OnIter);
VectSub(PnIter , Pn , JdThetaP);
VectSub(JdThetaP, dX , ErrVectP);
VectSub(OnIter , On , JdThetaO);
VectSub(JdThetaO, &dX[3], ErrVectO);
Err = sqrt ( ScalProd(ErrVectP,ErrVectP) + ScalProd(ErrVectO,ErrVectO) );
/* ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ */
//Calcul du nouveau vecteur d'entrée : division par deux pour fluidifier le mouvement **
// ..
dX[0] *=0.5;
dX[1] *=0.5;
dX[2] *=0.5;
dX[3] *=0.5;
dX[4] *=0.5;
dX[5] *=0.5;
/* ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ */
NoOfIter++;
} while ( (Err > _INNER_EPSILON_) && (NoOfIter < _MAX_INNER_ITER_) );
#ifdef _DEBUG
fprintf(stderr, "INNER loop: %d\n", NoOfIter);
#endif
/* **************************************************************************************************************************** */
}
void odomCb(const nav_msgs::Odometry::ConstPtr& msg)
{
float *vx;
double *z_target;
float *az;
double *time_begin;
bool *start_turn;
bool *quad_top_turn;
bool *is_turning;
Quaternionm myq;
double yaw = 0;
double pitch = 0;
double roll = 0;
myq.x = msg->pose.pose.orientation.x;
myq.y = msg->pose.pose.orientation.y;
myq.z = msg->pose.pose.orientation.z;
myq.w = msg->pose.pose.orientation.w;
GetEulerAngles(myq, yaw, pitch, roll);
if(msg->header.frame_id == "robot4/odom")
{
vx = &vx_4;
z_target = &z_target_4;
az = &az_4;
time_begin = &time_begin_4;
start_turn = &start_turn_4;
quad_top_turn = &quad_top_turn_4;
is_turning = &is_turning_4;
}
else if(msg->header.frame_id == "robot5/odom")
{
vx = &vx_5;
z_target = &z_target_5;
az = &az_5;
time_begin = &time_begin_5;
start_turn = &start_turn_5;
quad_top_turn = &quad_top_turn_5;
is_turning = &is_turning_5;
}
else if(msg->header.frame_id == "robot6/odom")
{
vx = &vx_6;
z_target = &z_target_6;
az = &az_6;
time_begin = &time_begin_6;
start_turn = &start_turn_6;
quad_top_turn = &quad_top_turn_6;
is_turning = &is_turning_6;
}
else if(msg->header.frame_id == "robot7/odom")
{
vx = &vx_7;
z_target = &z_target_7;
az = &az_7;
time_begin = &time_begin_7;
start_turn = &start_turn_7;
quad_top_turn = &quad_top_turn_7;
is_turning = &is_turning_7;
}
else if(msg->header.frame_id == "robot8/odom")
{
vx = &vx_8;
z_target = &z_target_8;
az = &az_8;
time_begin = &time_begin_8;
start_turn = &start_turn_8;
quad_top_turn = &quad_top_turn_8;
is_turning = &is_turning_8;
}
else if(msg->header.frame_id == "robot9/odom")
{
vx = &vx_9;
z_target = &z_target_9;
az = &az_9;
time_begin = &time_begin_9;
start_turn = &start_turn_9;
quad_top_turn = &quad_top_turn_9;
is_turning = &is_turning_9;
}
else if(msg->header.frame_id == "robot10/odom")
{
vx = &vx_10;
z_target = &z_target_10;
az = &az_10;
time_begin = &time_begin_10;
start_turn = &start_turn_10;
quad_top_turn = &quad_top_turn_10;
is_turning = &is_turning_10;
}
else if(msg->header.frame_id == "robot11/odom")
{
vx = &vx_11;
z_target = &z_target_11;
az = &az_11;
time_begin = &time_begin_11;
start_turn = &start_turn_11;
quad_top_turn = &quad_top_turn_11;
is_turning = &is_turning_11;
}
else if(msg->header.frame_id == "robot12/odom")
{
vx = &vx_12;
z_target = &z_target_12;
az = &az_12;
time_begin = &time_begin_12;
start_turn = &start_turn_12;
quad_top_turn = &quad_top_turn_12;
is_turning = &is_turning_12;
}
else if(msg->header.frame_id == "robot13/odom")
{
vx = &vx_13;
z_target = &z_target_13;
az = &az_13;
time_begin = &time_begin_13;
start_turn = &start_turn_13;
quad_top_turn = &quad_top_turn_13;
is_turning = &is_turning_13;
}
std::string Caller = "";
double diff = *z_target - yaw;
diff = fabs(diff);
double time_present = ros::Time::now().toSec();
//ROS_INFO("time_present: %lf, time_begin: %lf, roll: %lf",time_present,time_begin,diff);
if(diff > 0.13490 && *is_turning == true)
{
*time_begin = time_present;
*vx = 0;
*az = a_max;
}
else if(diff < 0.13490 && *is_turning == true)
{
*is_turning = false;
*vx = v_max;
*az = 0;
}
if(!*is_turning)
{
*vx = v_max;
*az = 0;
}
if((time_present - *time_begin) > wait_period)
{
angle_diff = 45;
calculate_turn = true;
Caller = "time";
}
if(*start_turn == true)
{
angle_diff = 180;
calculate_turn = true;
*start_turn = false;
Caller = "base";
}
if(*quad_top_turn == true)
{
angle_diff = 45;
calculate_turn = true;
*quad_top_turn = false;
Caller = "top";
}
if(calculate_turn)
{
*is_turning = true;
double turn_angle = (angle_diff*3.141592)/180;
*z_target = yaw - turn_angle;
if(*z_target > 3.14)
{
*z_target = 2*3.14 - *z_target;
}
else if(*z_target < -3.14)
{
*z_target = 2*3.14 + *z_target;
}
//ROS_INFO("target: %lf",z_target);
*time_begin = time_present;
calculate_turn = false;
*vx = -v_max*5;
}
publish_velocity(msg->header.frame_id);
}