/** * 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; }