#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; }