/**
* Performs eigenvector decomposition of an affinity matrix
*
* @param data the affinity matrix
* @param numDims the number of dimensions to consider when clustering
*/
SpectralClustering::SpectralClustering(Eigen::MatrixXd& data, int numDims):
mNumDims(numDims),
mNumClusters(0)
{
Eigen::MatrixXd Deg = Eigen::MatrixXd::Zero(data.rows(),data.cols());
// calc normalised laplacian
for ( int i=0; i < data.cols(); i++) {
Deg(i,i)=1/(sqrt((data.row(i).sum())) );
}
Eigen::MatrixXd Lapla = Deg * data * Deg;
Eigen::SelfAdjointEigenSolver<Eigen::MatrixXd> s(Lapla, true);
Eigen::VectorXd val = s.eigenvalues();
Eigen::MatrixXd vec = s.eigenvectors();
//sort eigenvalues/vectors
int n = data.cols();
for (int i = 0; i < n - 1; ++i) {
int k;
val.segment(i, n - i).maxCoeff(&k);
if (k > 0) {
std::swap(val[i], val[k + i]);
vec.col(i).swap(vec.col(k + i));
}
}
//choose the number of eigenvectors to consider
if (mNumDims < vec.cols()) {
mEigenVectors = vec.block(0,0,vec.rows(),mNumDims);
} else {
mEigenVectors = vec;
}
}
/**
* Generates an artificial topographyGrid of size numRows x numCols if no
* topographic data is available. Results are dumped into topographyGrid.
* @param topographyGrid A pointer to a zero-initialized Grid of size
* numRows x numCols.
* @param numRows The desired number of non-border rows in the resulting matrix.
* @param numCols The desired number of non-border cols in the resulting matrix.
*/
void simulatetopographyGrid(Grid* topographyGrid, int numRows, int numCols) {
Eigen::VectorXd refx = refx.LinSpaced(numCols, -2*M_PI, 2*M_PI);
Eigen::VectorXd refy = refx.LinSpaced(numRows, -2*M_PI, 2*M_PI);
Eigen::MatrixXd X = refx.replicate(1, numRows);
X.transposeInPlace();
Eigen::MatrixXd Y = refy.replicate(1, numCols);
// Eigen can only deal with two matrices at a time,
// so split the computation:
// topographyGrid = sin(X) * sin(Y) * abs(X) * abs(Y) -pi
Eigen::MatrixXd absXY = X.cwiseAbs().cwiseProduct(Y.cwiseAbs());
Eigen::MatrixXd sins = X.array().sin().cwiseProduct(Y.array().sin());
Eigen::MatrixXd temp;
temp.resize(numRows, numCols);
temp = absXY.cwiseProduct(sins);
// All this work to create a matrix of pi...
Eigen::MatrixXd pi;
pi.resize(numRows, numCols);
pi.setConstant(M_PI);
temp = temp - pi;
// And the final result.
topographyGrid->data.block(border, border, numRows, numCols) =
temp.block(0, 0, numRows, numCols);
// Ignore positive values.
topographyGrid->data =
topographyGrid->data.unaryExpr(std::ptr_fun(validateDepth));
topographyGrid->clearNA();
}
double GenericCell<dim, spacedim>::get_error_in_cell(
const TimeFunction<dim, dealii::Tensor<1, func_out_dim> > &func,
const Eigen::MatrixXd &modal_vector,
const double &time)
{
double error = 0;
const std::vector<dealii::Point<dim> > &points_loc =
cell_quad_fe_vals->get_quadrature_points();
const std::vector<double> &JxWs = cell_quad_fe_vals->get_JxW_values();
unsigned n_unknowns = the_elem_basis->n_polys;
assert(points_loc.size() == JxWs.size());
assert(modal_vector.rows() == func_out_dim * n_unknowns);
// assert((long int)points_loc.size() == mode_to_Qpoint_matrix.rows());
std::vector<Eigen::MatrixXd> values_at_Nodes(func_out_dim);
for (unsigned i_dim = 0; i_dim < func_out_dim; ++i_dim)
values_at_Nodes[i_dim] = the_elem_basis->get_dof_vals_at_quads(
modal_vector.block(i_dim * n_unknowns, 0, n_unknowns, 1));
for (unsigned i_point = 0; i_point < JxWs.size(); ++i_point)
{
dealii::Tensor<1, func_out_dim> temp_val;
for (unsigned i_dim = 0; i_dim < func_out_dim; ++i_dim)
{
temp_val[i_dim] = values_at_Nodes[i_dim](i_point, 0);
}
dealii::Tensor<1, func_out_dim> func_val =
func.value(points_loc[i_point], points_loc[i_point], time);
error += (func_val - temp_val) * (func_val - temp_val) * JxWs[i_point];
}
return error;
}
void TaskSolverDmpArm2D::performRollout(const Eigen::VectorXd& sample, const Eigen::VectorXd& task_parameters, Eigen::MatrixXd& cost_vars) const
{
TaskSolverDmp::performRollout(sample, task_parameters, cost_vars);
// cost_vars_angles structure is (without the link positions!)
//
// time joint angles (e.g. n_dofs = 3) forcing term link positions (e.g. 3+1)
// ____ __________________________________ __________ _________________________
// t | a a a | ad ad ad | add add add | f f f | x y | x y | x y | x y |
//
// We now compute the link positions and add them to the end
int n_dofs = dmp_->dim_orig();
int n_time_steps = cost_vars.rows();
assert(cost_vars.cols() == 1+4*n_dofs);
// Make room for link positions, i.e. 2 * (n_links+1)
cost_vars.conservativeResize(n_time_steps, 1 + 4*n_dofs + 2*(n_dofs+1));
MatrixXd angles = cost_vars.block(0,1,n_time_steps,n_dofs);
MatrixXd link_positions;
anglesToLinkPositions(angles,link_positions);
// Add the link positions to the right side of the cost_vars matrix
cost_vars.rightCols(2*(n_dofs+1)) = link_positions;
}
/**
* @brief hqp_wrapper::updObstacle Updates obstacles' matrices
* @param levelName Level's ID
* @param Jac Jacobian (6xNc)
* @param n Normalised Vector between end effector and obstacles' position
* @param b scalar b, n*J < b
*/
void hqp_wrapper::updObstacle(std::string levelName, const Eigen::MatrixXd Jac, const Eigen::Vector3d n, const double b ){
int indx = leveNameMap[levelName];
this->B[indx][0] = soth::Bound(b, soth::Bound::BOUND_INF);
this->J[indx] = n.transpose()*Jac.block(0,0,3,this->NC);
// std::cout << "J[indx]:\n" <<J[indx] <<"\n";
}
IGL_INLINE void igl::ViewerData::set_uv(const Eigen::MatrixXd& UV_V, const Eigen::MatrixXi& UV_F)
{
set_face_based(true);
V_uv = UV_V.block(0,0,UV_V.rows(),2);
F_uv = UV_F;
dirty |= DIRTY_UV;
}
void BallJointConstraint::updateDynamics(Eigen::MatrixXd & _J1, Eigen::VectorXd & _C, Eigen::VectorXd & _CDot, int _rowIndex) {
getJacobian();
_J1.block(_rowIndex, 0, 3, mBodyNode1->getSkeleton()->getNumGenCoords()) = mJ1;
Eigen::Vector3d worldP1 = mBodyNode1->getWorldTransform() * mOffset1;
Eigen::VectorXd qDot1 = mBodyNode1->getSkeleton()->get_dq();
_C.segment(_rowIndex, 3) = worldP1 - mOffset2;
_CDot.segment(_rowIndex, 3) = mJ1 * qDot1;
}
/**
* @brief hqp_wrapper::addObstacle
* @param levelName String to be used as ID
* @param Jac Jacobian (6xNc), only the first 3 rows (position) is used currently
* @param n Normalised Vector between end effector and obstacles' position
* @param b scalar b, n*J < b
*/
void hqp_wrapper::addObstacle(std::string levelName, const Eigen::MatrixXd Jac, const Eigen::Vector3d n, double b){
Eigen::MatrixXd Jtmp;
Jtmp = n.transpose()*Jac.block(0,0,3,this->NC);
Eigen::Matrix<double,1,1> B;
B(0) = b;
addStage(levelName,Jtmp,B,soth::Bound::BOUND_INF);
std::cout <<"Adding new element:" << levelName <<", at:" << leveNameMap[levelName]<<"\n";
}
void addMatrixBlock( Eigen::MatrixXd &matrix, const int block_length ){
assert( matrix.rows()==matrix.cols() );
int old_size = matrix.rows();
matrix.conservativeResize( old_size+block_length, old_size+block_length );
/*
O: old
Nx: newly initalized
O N1
N2 N1
*/
// N1
matrix.block( 0, old_size, old_size+block_length, block_length ) = Eigen::MatrixXd::Zero( old_size+block_length, block_length);
// N2
matrix.block( old_size, 0, block_length, old_size ) = Eigen::MatrixXd::Zero( block_length, old_size);
}
Eigen::MatrixXd padMatrix(Eigen::MatrixXd d, int r)
{
using namespace Eigen;
MatrixXd out = MatrixXd::Zero(d.rows()+2*r, d.cols()+2*r);
out.block(r, r, d.rows(), d.cols()) = d;
out.block(r, 0, d.rows(), r) =
d.block(0, 0, d.rows(), r).rowwise().reverse();
out.block(r, d.cols()+r, d.rows(), r) =
d.block(0, d.cols()-r, d.rows(), r).rowwise().reverse();
out.block(0, 0, r, out.cols()) =
out.block(r, 0, r, out.cols()).colwise().reverse();
out.block(d.rows()+r, 0, r, out.cols()) =
out.block(out.rows()-r, 0, r, out.cols()).colwise().reverse();
return out;
}
//! Function to get the concatenated state transition matrices for each arc and sensitivity matrix at a given time.
Eigen::MatrixXd MultiArcCombinedStateTransitionAndSensitivityMatrixInterface::getFullCombinedStateTransitionAndSensitivityMatrix(
const double evaluationTime )
{
Eigen::MatrixXd combinedStateTransitionMatrix = Eigen::MatrixXd::Zero(
stateTransitionMatrixSize_, numberOfStateArcs_ * stateTransitionMatrixSize_ + sensitivityMatrixSize_ );
int currentArc = lookUpscheme_->findNearestLowerNeighbour( evaluationTime );
// Set Phi and S matrices of current arc.
combinedStateTransitionMatrix.block( 0, currentArc * stateTransitionMatrixSize_, stateTransitionMatrixSize_, stateTransitionMatrixSize_ ) =
stateTransitionMatrixInterpolators_.at( currentArc )->interpolate( evaluationTime );
combinedStateTransitionMatrix.block(
0, numberOfStateArcs_ * stateTransitionMatrixSize_, stateTransitionMatrixSize_, sensitivityMatrixSize_ ) =
sensitivityMatrixInterpolators_.at( currentArc )->interpolate( evaluationTime );
return combinedStateTransitionMatrix;
}
void calc()
{
Eigen::MatrixXd M_inv1 = M1.inverse();
Eigen::MatrixXd M_inv2 = M2.inverse();
Eigen::MatrixXd Jv1 = J1.block(0, 0, 3, J1.cols());
Eigen::MatrixXd Jv2 = J2.block(0, 0, 3, J2.cols());
Eigen::MatrixXd Lambda_inv1 = Jv1 * M_inv1 * Jv1.transpose();
Eigen::MatrixXd Lambda_inv2 = Jv2 * M_inv2 * Jv2.transpose();
Eigen::MatrixXd Lambda1 = Lambda_inv1.inverse();
Eigen::MatrixXd Lambda2 = Lambda_inv2.inverse();
Eigen::MatrixXd J_dyn_inv1 = M_inv1 * Jv1.transpose() * Lambda1;
Eigen::MatrixXd J_dyn_inv2 = M_inv2 * Jv2.transpose() * Lambda2;
Eigen::MatrixXd I = Eigen::MatrixXd::Identity(M_inv1.rows(), M_inv1.rows());
Eigen::MatrixXd N1 = I - J_dyn_inv1 * Jv1;
Eigen::MatrixXd N2 = I - J_dyn_inv2 * Jv2;
//std::cout << N.transpose() << std::endl << std::endl;
}
Eigen::MatrixXd grad(const Eigen::VectorXd& x, const GP& gp) const
{
Eigen::MatrixXd grad = Eigen::MatrixXd::Zero(_tr.rows(), _h_params.size());
Eigen::VectorXd m = _mean_function(x, gp);
for (int i = 0; i < _tr.rows(); i++) {
grad.block(i, i * _tr.cols(), 1, _tr.cols() - 1) = m.transpose();
grad(i, (i + 1) * _tr.cols() - 1) = 1;
}
return grad;
}
/*! \fn inline void symmetryPacking(std::vector<Eigen::MatrixXd> & blockedMatrix, const Eigen::MatrixXd & fullMatrix, int nrBlocks, int dimBlock)
* \param[out] blockedMatrix the result of packing fullMatrix
* \param[in] fullMatrix the matrix to be packed
* \param[in] dimBlock the dimension of the square blocks
* \param[in] nrBlocks the number of square blocks
*/
inline void symmetryPacking(std::vector<Eigen::MatrixXd> & blockedMatrix,
const Eigen::MatrixXd & fullMatrix, int dimBlock, int nrBlocks)
{
// This function packs the square block diagonal fullMatrix with nrBlocks of dimension dimBlock
// into a std::vector<Eigen::MatrixXd> containing nrBlocks square matrices of dimenion dimBlock.
int j = 0;
for (int i = 0; i < nrBlocks; ++i) {
blockedMatrix.push_back(fullMatrix.block(j, j, dimBlock, dimBlock));
j += dimBlock;
}
}
void drawField(igl::viewer::Viewer &viewer,
const Eigen::MatrixXd &field,
const Eigen::RowVector3d &color)
{
for (int n=0; n<2; ++n)
{
Eigen::MatrixXd VF = field.block(0,n*3,F.rows(),3);
Eigen::VectorXd c = VF.rowwise().norm();
viewer.data.add_edges(B - global_scale*VF, B + global_scale*VF , color);
}
}
Eigen::Quaterniond MainWindow::rotation2quaternion( Eigen::MatrixXd rotation )
{
Eigen::Matrix3d _rotation3d;
_rotation3d = rotation.block( 0 , 0 , 3 , 3 );
Eigen::Quaterniond _quaternion;
_quaternion = _rotation3d;
return _quaternion;
}
Eigen::VectorXd BicZ_j_cpp(Eigen::MatrixXd X,Eigen::MatrixXd Z,Eigen::VectorXd Bic_vide_vect,Eigen::VectorXd BicOld,double methode,int j)
{
int size=X.cols();
int sizerow = X.rows();
Eigen::VectorXd BicRes(size);
Eigen::MatrixXd mat_var_a_droite;
int k;
//debut de la boucle colonne de Z
BicRes=BicOld;
//colonne de Z sur laquelle on va travailler (variable a gauche)
Eigen::VectorXd colZ=Z.block(0,j,size,1);
int newsize=colZ.sum();
//si la somme de la colonne=0 alors on appelle mixmod
if(newsize==0)
{
BicRes(j)=Bic_vide_vect(j);
}
else
{
//calcul du nouveau BIC
mat_var_a_droite.resize(sizerow,newsize);
k=0;
//on echerche l'emplacement des 1 dans le vecteur colonne (variables a droite)
for(int p=0;p<size;p++)
{
if (colZ(p)==1)
{
//on regroupe les variables a droite dans une matrice
mat_var_a_droite.block(0,k,sizerow,1)=X.block(0,p,sizerow,1);
k=k+1;
}//fin if
}//fin for j
//calcul de son BIC en appellant OLS (les parametres sont : Y=X[,colonne],X=X[,matrice des variables à droite])
BicRes(j)=BicLoc_cpp(mat_var_a_droite,X.block(0,j,sizerow,1),"T",methode);
}//fin else
return BicRes;
}
void deleteMatrixBlock( Eigen::MatrixXd &matrix, const int block_start_index, const int block_length ){
assert( matrix.rows()==matrix.cols() );
assert( matrix.rows()>=block_start_index+block_length );
assert( block_start_index>=0 );
int size = matrix.rows();
/*
R: remain
D: delete
Mx: move
R D M1
D D D
M2 D M3
*/
int size_to_move = size-block_start_index-block_length;
int move_from = block_start_index+block_length;
// M1
matrix.block( 0, block_start_index, block_start_index, size_to_move) = matrix.block( 0, move_from, block_start_index, size_to_move );
// M2
matrix.block( block_start_index, 0, size_to_move, block_start_index ) = matrix.block( move_from, 0, size_to_move, block_start_index );
// M3
matrix.block( block_start_index, block_start_index, size_to_move, size_to_move) = matrix.block( move_from, move_from, size_to_move, size_to_move );
matrix.conservativeResize( size-block_length, size-block_length );
}
void JointControl::setGoal(const Eigen::MatrixXd& qd)
{
if(qd.rows() != mnp_->getDOF())
{
std::stringstream msg;
msg << "qd.rows() != mnp_->getDOF()" << std::endl
<< " qd.rows : " << qd.rows() << std::endl
<< " mnp_->dof : " << mnp_->getDOF();
throw ahl_utils::Exception("JointControl::setGoal", msg.str());
}
qd_ = qd.block(0, 0, qd.rows(), 1);
}
// ONLY USE WHEN MASS MATRIX CONTAINS FREE BASE TRANSLATION AND ROTATION
// Since the translation and rotation are switched
// WBI order [T R Q]
// ocra order [R T Q]
/* static */ bool ocraWbiConversions::wbiToOcraMassMatrix(int qdof, const Eigen::MatrixXd &M_wbi, Eigen::MatrixXd &M_ocra)
{
int dof = qdof + DIM_T + DIM_R;
if(dof != M_wbi.cols() || dof != M_wbi.rows() || dof != M_ocra.rows() || dof != M_ocra.cols())
{
std::cout<<"ERROR: Input and output matrices - Is the model free root?" <<std::endl;
return false;
}
// ORIGINAL MATRIX BLOCKS
Eigen::MatrixXd m11(DIM_T, DIM_T);
m11 = M_wbi.block(0, 0, DIM_T, DIM_T);
Eigen::MatrixXd m12(DIM_T, DIM_R);
m12 = M_wbi.block(0, DIM_T, DIM_T, DIM_R);
Eigen::MatrixXd m13(DIM_T, qdof);
m13 = M_wbi.block(0, DIM_T+DIM_R, DIM_T, qdof);
Eigen::MatrixXd m21(DIM_R, DIM_T);
m21 = M_wbi.block(DIM_T, 0, DIM_R, DIM_T);
Eigen::MatrixXd m22(DIM_R, DIM_R);
m22 = M_wbi.block(DIM_T, DIM_T, DIM_R, DIM_R);
Eigen::MatrixXd m23(DIM_R, qdof);
m23 = M_wbi.block(DIM_T, DIM_T+DIM_R, DIM_R, qdof);
Eigen::MatrixXd m31(qdof, DIM_T);
m31 = M_wbi.block(DIM_T+DIM_R, 0, qdof, DIM_T);
Eigen::MatrixXd m32(qdof, DIM_R);
m32 = M_wbi.block(DIM_T+DIM_R, DIM_T, qdof, DIM_R);
Eigen::MatrixXd m33(qdof, qdof);
m33 = M_wbi.block(DIM_T+DIM_R, DIM_T+DIM_R, qdof, qdof);
M_ocra << m22, m21, m23,
m12, m11, m13,
m32, m31, m33;
return true;
}
Eigen::VectorXd flattenSymmetric(Eigen::MatrixXd symm)
{
//todo: other data types?
//todo: safety/square matrix checking?
int num_entries = symm.rows() * (symm.rows() + 1) / 2;
Eigen::VectorXd flat(num_entries);
int count = 0;
for (int i = 0; i < symm.cols(); i++) {
int ltcsz = symm.cols() - i;
flat.block(count, 0, ltcsz, 1) = symm.block(i, i, ltcsz, 1);
count += ltcsz;
}
return flat;
}
void calc()
{
//std::cout << "M : " << std::endl << M.inverse() << std::endl;
// << "J : " << std::endl << J << std::endl << std::endl;
Eigen::MatrixXd M_inv = M.inverse();
Eigen::MatrixXd Jv = J.block(0, 0, 3, J.cols());
Eigen::MatrixXd Lambda_inv = Jv * M_inv * Jv.transpose();
Eigen::MatrixXd Lambda = Lambda_inv.inverse();
Eigen::MatrixXd J_dyn_inv = M_inv * Jv.transpose() * Lambda;
Eigen::MatrixXd I = Eigen::MatrixXd::Identity(15, 15);
Eigen::MatrixXd N = I - J_dyn_inv * Jv;
//std::cout << N.transpose() << std::endl << std::endl;
}
void BranchList::smoothOrientations()
{
for (int i = 0; i < mBranches.size(); i++)
{
Eigen::MatrixXd orientations = mBranches[i]->getOrientations();
Eigen::MatrixXd newOrientations(orientations.rows(),orientations.cols());
int numberOfColumns = orientations.cols();
for (int j = 0; j < numberOfColumns; j++)
{
newOrientations.col(j) = orientations.block(0,std::max(j-2,0),orientations.rows(),std::min(5,numberOfColumns-j)).rowwise().mean(); //smoothing
newOrientations.col(j) = newOrientations.col(j) / newOrientations.col(j).norm(); // normalizing
}
mBranches[i]->setOrientations(newOrientations);
}
}
void Verify::write_to_file()
{
int idx = nr_links.size()-1;
Eigen::MatrixXd result = Eigen::MatrixXd::Zero(idx, 23);
result.block(0,0,idx,1) = link_mean;
result.block(0,1,idx,1) = R;
result.block(0,2,idx,1) = R_var;
result.block(0,3,idx,1) = R_theo;
result.block(0,4,idx,1) = Rg;
result.block(0,5,idx,1) = Rg_var;
result.block(0,6,idx,1) = Rg_theo;
result.block(0,21,idx,1) = link_mean;
std::ofstream file;
file.open(_outfile, std::fstream::out | std::fstream::trunc);
if(file.is_open()){
file << result << std::endl;
}else{
std::cerr << "Failed to open " << std::string(_outfile) << std::endl;
}
file.close();
}
/// \brief evaluate the jacobians
void ErrorTermTransformation::evaluateJacobiansImplementation(JacobianContainer & _jacobians) const
{
sm::kinematics::RotationVector rotVector;
Eigen::MatrixXd J = Eigen::MatrixXd::Identity(6,6);
J.block(0,3,3,3) = sm::kinematics::crossMx(_errorMatrix.block(0,3,3,1));
J.block(3,3,3,3) = rotVector.parametersToInverseSMatrix(sm::kinematics::R2AxisAngle(_errorMatrix.block(0,0,3,3)));
if (_debug == 1)
{
std::cout << "errorMatrix:\n" << _errorMatrix << "\n";
std::cout << "error:\n" << error() << "\n";
std::cout << "J:\n" << J << "\n";
}
_T.evaluateJacobians(_jacobians, J);
}
//! Function to filter tidal corrections based on RSS amplitude criteria.
std::pair< Eigen::MatrixXd, Eigen::MatrixXd > filterRawDataForAmplitudes(
const Eigen::MatrixXd rawAmplitudes,
const Eigen::MatrixXd rawFundamentalArgumentMultipliers,
const double minimumAmplitude )
{
// Get number of amplitudes per entry.
int numberOfAmplitudes = rawAmplitudes.cols( );
// Initialize return matrices
int numberOfAcceptedAmplitudes = 0;
Eigen::MatrixXd filteredAmplitudes;
filteredAmplitudes.resize( numberOfAcceptedAmplitudes, numberOfAmplitudes );
Eigen::MatrixXd filteredFundamentalArgumentMultipliers;
filteredFundamentalArgumentMultipliers.resize( numberOfAcceptedAmplitudes, 6 );
// Iterate over all entries, calculate RSS amplitude and accept or reject entry.
double rssAmplitude = 0.0;
for( int i = 0; i < rawAmplitudes.rows( ); i++ )
{
// Calculate RSS of amplitude.
rssAmplitude = 0.0;
for( int j =0; j < numberOfAmplitudes; j++ )
{
rssAmplitude += rawAmplitudes( i, j ) * rawAmplitudes( i, j );
}
rssAmplitude = std::sqrt( rssAmplitude );
// If RSS amplitude is sufficiently large, accept value.
if( rssAmplitude > minimumAmplitude )
{
if( numberOfAcceptedAmplitudes == 0 )
{
numberOfAcceptedAmplitudes++;
filteredAmplitudes.resize( 1, numberOfAmplitudes );
filteredAmplitudes = rawAmplitudes.block( i, 0, 1, numberOfAmplitudes );
filteredFundamentalArgumentMultipliers.resize( 1, 6 );
filteredFundamentalArgumentMultipliers = rawFundamentalArgumentMultipliers.block( i, 0, 1, 6 );
}
else
{
numberOfAcceptedAmplitudes++;
filteredAmplitudes.conservativeResize( numberOfAcceptedAmplitudes, numberOfAmplitudes );
filteredAmplitudes.block( numberOfAcceptedAmplitudes - 1, 0, 1, numberOfAmplitudes ) =
rawAmplitudes.block( i, 0, 1, numberOfAmplitudes );
filteredFundamentalArgumentMultipliers.conservativeResize( numberOfAcceptedAmplitudes, 6 );
filteredFundamentalArgumentMultipliers.block( numberOfAcceptedAmplitudes - 1, 0, 1, 6 ) =
rawFundamentalArgumentMultipliers.block( i, 0, 1, 6 );
}
}
}
return std::pair< Eigen::MatrixXd, Eigen::MatrixXd >( filteredAmplitudes, filteredFundamentalArgumentMultipliers );
}
void drawConstraints(igl::viewer::Viewer &viewer)
{
for (int n=0; n<2; ++n)
{
Eigen::MatrixXd Bc = igl::slice(B, b, 1);
Eigen::MatrixXd color;
color.setZero(b.rows(),3);
color.col(2).setOnes();
for (int i =0; i<b.rows(); ++i)
if (blevel[i] ==1 && n>0)
color.row(i)<<0.7,0.7,0.7;
// Eigen::RowVector3d color; color<<0.5,0.5,0.5;
viewer.data.add_edges(Bc - global_scale*bc.block(0,n*3,bc.rows(),3), Bc + global_scale*bc.block(0,n*3,bc.rows(),3) , color);
}
}
bool demeanPointCloud(std::vector<Point3d>& cloud,
Eigen::Vector4d& centroid, Eigen::MatrixXd &cloud_out)
{
size_t npts = cloud.size();
cloud_out = Eigen::MatrixXd::Zero(4, npts); // keep the data aligned
for(size_t i=0; i< npts; ++i)
{
Eigen::Vector4d pt;
pt << cloud[i].x, cloud[i].y, cloud[i].z, 0.0;
cloud_out.block<4,1>(0, i) = pt - centroid;
// Make sure zero out the 4th dimension (1 row, N cloumns)
cloud_out.block(3, 0, 1, npts).setZero();
}
}
bool KinematicsMetrics::getManipulabilityEllipsoid(const robot_state::RobotState &state,
const robot_model::JointModelGroup *joint_model_group,
Eigen::MatrixXcd &eigen_values,
Eigen::MatrixXcd &eigen_vectors) const
{
// state.getJacobian() only works for chain groups.
if(!joint_model_group->isChain())
{
return false;
}
Eigen::MatrixXd jacobian = state.getJacobian(joint_model_group);
Eigen::MatrixXd matrix = jacobian*jacobian.transpose();
Eigen::EigenSolver<Eigen::MatrixXd> eigensolver(matrix.block(0, 0, 3, 3));
eigen_values = eigensolver.eigenvalues();
eigen_vectors = eigensolver.eigenvectors();
return true;
}
Eigen::MatrixXd cIKSolver::BuildJacob(const Eigen::MatrixXd& joint_mat, const Eigen::VectorXd& pose, const Eigen::MatrixXd& cons_mat)
{
int num_dof = cKinTree::GetNumDof(joint_mat);
int cons_dim = CountConsDim(cons_mat);
int num_cons = static_cast<int>(cons_mat.rows());
Eigen::MatrixXd J = Eigen::MatrixXd(cons_dim, num_dof);
int r = 0;
for (int c = 0; c < num_cons; ++c)
{
const tConsDesc& curr_cons = cons_mat.row(c);
Eigen::MatrixXd curr_J = BuildJacob(joint_mat, pose, curr_cons);
int curr_dim = static_cast<int>(curr_J.rows());
J.block(r, 0, curr_dim, num_dof) = curr_J;
r += curr_dim;
}
return J;
}