#undef _
#undef ____
#define R_(a,b) [PIPE_FORMAT_##a] = { \
.hw = NV30_3D_RT_FORMAT_COLOR_##b, \
}
#define Z_(a,b) [PIPE_FORMAT_##a] = { \
.hw = NV30_3D_RT_FORMAT_ZETA_##b, \
}
const struct nv30_format
nv30_format_table[PIPE_FORMAT_COUNT] = {
R_(B5G5R5X1_UNORM , X1R5G5B5 ),
R_(B5G6R5_UNORM , R5G6B5 ),
R_(B8G8R8X8_UNORM , X8R8G8B8 ),
R_(B8G8R8A8_UNORM , A8R8G8B8 ),
Z_(Z16_UNORM , Z16 ),
Z_(X8Z24_UNORM , Z24S8 ),
Z_(S8_UINT_Z24_UNORM , Z24S8 ),
R_(R16G16B16A16_FLOAT, A16B16G16R16_FLOAT),
R_(R32G32B32A32_FLOAT, A32B32G32R32_FLOAT),
R_(R32_FLOAT , R32_FLOAT ),
};
#define _(a,b,c) [PIPE_FORMAT_##a] = { \
.hw = NV30_3D_VTXFMT_TYPE_##b | ((c) << NV30_3D_VTXFMT_SIZE__SHIFT) \
}
const struct nv30_vtxfmt
nv30_vtxfmt_table[PIPE_FORMAT_COUNT] = {
_(R8_UNORM , U8_UNORM , 1),
_(R8G8_UNORM , U8_UNORM , 2),
_(R8G8B8_UNORM , U8_UNORM , 3),
geometry_msgs::Twist generate_smooth_trajectory(geometry_msgs::Twist p_ini, geometry_msgs::Twist p_fin, ros::Publisher pub_pos, double tiempo, bool respecto_robot, geometry_msgs::Twist vel_ant){
//Encontrar las posiciones de la trayectoria con ayuda de un polinomio de quinto orden
double p_ini_x_;
double p_ini_y_;
double a_ini_z_;
double p_fin_x_;
double p_fin_y_;
double a_fin_z_;
double r_time_;
p_ini_x_ = p_ini.linear.x;//10;
p_ini_y_ = p_ini.linear.y;//10;
a_ini_z_ = p_ini.angular.z;//10;
p_fin_x_ = p_fin.linear.x;///10;
p_fin_y_ = p_fin.linear.y;//10;
a_fin_z_ = p_fin.angular.z;//10;
r_time_ = p_fin.angular.x;
Eigen::MatrixXf Mat(6,6);
Mat <<0,0,0,0,0,1,
pow(r_time_,5),pow(r_time_,4),pow(r_time_,3),pow(r_time_,2),r_time_,1,
0,0,0,0,1,0,
5*pow(r_time_,4),4*pow(r_time_,3),3*pow(r_time_,2),2*r_time_,1,0,
0,0,0,2,0,0,
20*pow(r_time_,3),12*pow(r_time_,2),6*r_time_,2,0,0;
//if (!respecto_robot)
// std::cout << "Matriz de polinomio quinto orden" <<Mat << std::endl<< r_time_<<std::endl<<tiempo<<std::endl;
Eigen::MatrixXf X_(6,1);
X_ <<p_ini_x_,p_fin_x_,vel_ant.linear.x,0,0,0;
Eigen::MatrixXf Y_(6,1);
Y_ <<p_ini_y_,p_fin_y_,vel_ant.linear.y,0,0,0;
Eigen::MatrixXf Z_(6,1);
Z_ <<a_ini_z_,a_fin_z_,vel_ant.angular.z,0,0,0;
//std::cout << "Vector X" <<X_ << std::endl;
Eigen::MatrixXf coeff_X(6,1);
coeff_X = Mat.colPivHouseholderQr().solve(X_);
//std::cout << "Vector X" <<coeff_X << std::endl;
Eigen::MatrixXf coeff_Y(6,1);
coeff_Y = Mat.colPivHouseholderQr().solve(Y_);
Eigen::MatrixXf coeff_Z(6,1);
coeff_Z = Mat.colPivHouseholderQr().solve(Z_);
double i =.02;// 1/rate_hz;
geometry_msgs::Twist pos_aux,vel_aux;
//pos_aux.linear.x =coeff_X(0,0)*pow(i,5)+coeff_X(1,0)*pow(i,4)+coeff_X(2,0)*pow(i,3)+coeff_X(3,0)*pow(i,2)+coeff_X(4,0)*i+coeff_X(5,0);
pos_aux.linear.x =coeff_X(0,0)*pow(i,5)+
coeff_X(1,0)*pow(i,4)+
coeff_X(2,0)*pow(i,3)+
coeff_X(3,0)*pow(i,2)+
coeff_X(4,0)*i+
coeff_X(5,0);
pos_aux.linear.y =coeff_Y(0,0)*pow(i,5)+coeff_Y(1,0)*pow(i,4)+coeff_Y(2,0)*pow(i,3)+coeff_Y(3,0)*pow(i,2)+coeff_Y(4,0)*i+coeff_Y(5,0);
pos_aux.angular.z=coeff_Z(0,0)*pow(i,5)+coeff_Z(1,0)*pow(i,4)+coeff_Z(2,0)*pow(i,3)+coeff_Z(3,0)*pow(i,2)+coeff_Z(4,0)*i+coeff_Z(5,0);
/*vel_aux.linear.x = 5*coeff_X(0,0)*pow(i,4)
+4*coeff_X(1,0)*pow(i,3)
+3*coeff_X(2,0)*pow(i,2)
+2*coeff_X(3,0)*i
+coeff_X(4,0);
vel_aux.linear.y = 5*coeff_Y(0,0)*pow(i,4)
+4*coeff_Y(1,0)*pow(i,3)
+3*coeff_Y(2,0)*pow(i,2)
+2*coeff_Y(3,0)*i
+coeff_Y(4,0);
vel_aux.angular.z =5*coeff_Z(0,0)*pow(i,4)
+4*coeff_Z(1,0)*pow(i,3)
+3*coeff_Z(2,0)*pow(i,2)
+2*coeff_Z(3,0)*i
+coeff_Z(4,0);*/
//vel_aux.angular.z = 0;
// Opcion 1
vel_aux.linear.x = (p_fin_x_-p_ini_x_)/(r_time_-tiempo);
vel_aux.linear.y = (p_fin_y_-p_ini_y_)/(r_time_-tiempo);
//vel_aux.angular.z = (a_fin_z_-a_ini_z_)/(r_time_-tiempo)*10;
//vel_aux.linear.x = (p_fin_x_-p_ini_x_)/(r_time_);
//vel_aux.linear.y = (p_fin_y_-p_ini_y_)/(r_time_);
vel_aux.angular.z = 0;
//Opcion 2
/*vel_aux.linear.x = abs(pos_aux.linear.x-p_fin_x_)/(1/(rate_hz));
vel_aux.linear.y = abs(pos_aux.linear.y-p_fin_y_)/(1/(rate_hz));
vel_aux.angular.z = 0;*/
if (respecto_robot){//Publicar la posicion respecto al mundo para tener una idea con el
pos_aux = destransformacion_homogenea(pos_aux, goalie_position.angular.z, goalie_position);
pub_pos.publish(pos_aux);
}
/* ROS_INFO_STREAM("pos esperada sin transf:"
<<" linear X ="<<pos_aux.linear.x
<<" linear Y ="<<pos_aux.linear.y
<<" angular ="<<pos_aux.angular.z);*/
/*}
else
{ Esto es respecto al mundo, no sirve mas que para saber que si se esta haciendo bien la transformacion lineal
pos_aux = destransformacion_homogenea(pos_aux, goalie_position.angular.z, goalie_position);
pub_pos.publish(pos_aux);
ROS_INFO_STREAM("pos final esp:"
<<" linear X ="<<p_fin_x_
<<" linear Y ="<<p_fin_y_
<<" angular ="<<a_fin_z_);
ROS_INFO_STREAM("pos inicial esp:"
<<" linear X ="<<p_ini_x_
<<" linear Y ="<<p_ini_y_
<<" angular ="<<a_ini_z_);
}*/
return vel_aux;
}
