std::vector<std::string> ThrusterAllocator::initControl(ros::NodeHandle &nh, double map_threshold)
{
std::vector<std::string> controlled_axes;
const std::vector<std::string> axes{"x", "y", "z", "roll", "pitch", "yaw"};
for(size_t axis = 0; axis < 6; axis++)
{
if(max_wrench[axis] > 1e-6)
{
char param[FILENAME_MAX];
sprintf(param, "controllers/%s", axes[axis].c_str());
if(nh.hasParam(param))
{
controlled_axes.push_back(axes[axis]);
// push to position PID
sprintf(param, "controllers/%s/position/i_clamp", axes[axis].c_str());
nh.setParam(param, max_wrench[axis]);
// push to velocity PID
sprintf(param, "controllers/%s/velocity/i_clamp", axes[axis].c_str());
nh.setParam(param, max_wrench[axis]);
}
}
}
// threshold on map before pseudo-inverse
for(int r = 0; r < 6; ++r)
{
for(int c = 0; c < map.cols(); ++c)
{
if(std::abs(map(r,c)) < map_threshold)
map(r,c) = 0;
}
}
inverse_map.resize(map.cols(), map.rows());
Eigen::JacobiSVD<Eigen::MatrixXd> svd_M(map, Eigen::ComputeThinU | Eigen::ComputeThinV);
Eigen::VectorXd dummy_in(map.rows());
Eigen::VectorXd dummy_out(map.cols());
unsigned int i,j;
for(i=0;i<map.rows();++i)
{
dummy_in.setZero();
dummy_in(i) = 1;
dummy_out = svd_M.solve(dummy_in);
for(j = 0; j<map.cols();++j)
inverse_map(j,i) = dummy_out(j);
}
return controlled_axes;
}
Point FEInterface::inverse_map (const unsigned int dim,
const FEType& fe_t,
const Elem* elem,
const Point& p,
const Real tolerance,
const bool secure)
{
#ifdef LIBMESH_ENABLE_INFINITE_ELEMENTS
if ( is_InfFE_elem(elem->type()) )
return ifem_inverse_map(dim, fe_t, elem, p,tolerance, secure);
#endif
fe_with_vec_switch(inverse_map(elem, p, tolerance, secure));
libmesh_error();
return Point();
}
void FEInterface::inverse_map (const unsigned int dim,
const FEType& fe_t,
const Elem* elem,
const std::vector<Point>& physical_points,
std::vector<Point>& reference_points,
const Real tolerance,
const bool secure)
{
const std::size_t n_pts = physical_points.size();
// Resize the vector
reference_points.resize(n_pts);
if (n_pts == 0)
{
libMesh::err << "WARNING: empty vector physical_points!"
<< std::endl;
libmesh_here();
return;
}
#ifdef LIBMESH_ENABLE_INFINITE_ELEMENTS
if ( is_InfFE_elem(elem->type()) )
{
ifem_inverse_map(dim, fe_t, elem, physical_points, reference_points, tolerance, secure);
return;
// libMesh::err << "ERROR: Not implemented!"
// << std::endl;
// libmesh_error();
}
#endif
void_fe_with_vec_switch(inverse_map(elem, physical_points, reference_points, tolerance, secure));
libmesh_error();
return;
}
// returns values of normalized Legendre polynomials on (a, b), for
// an arbitrary point 'x'
double legendre_val_phys_plot(int i, double a, double b, double x) { // x \in (a, b)
double norm_const = sqrt(2/(b-a));
return norm_const*legendre_val_ref(inverse_map(a, b, x), i);
}
// returns derivatives of normalized Legendre polynomials on (a, b), for
// an arbitrary point 'x'
double legendre_der_phys_plot(int i, double a, double b, double x) { // x \in (a, b)
double norm_const = sqrt(2/(b-a));
norm_const *= 2./(b-a); // to account for interval stretching/shortening
return norm_const*legendre_der_ref(inverse_map(a, b, x), i);
}
