Eigen::VectorXd LDAModel::predict(const vector<double> & substance, bool transform) { if (sigma_.cols() == 0) { throw Exception::InconsistentUsage(__FILE__, __LINE__, "Model must be trained before it can predict the activitiy of substances!"); } // search class to which the given substance has the smallest distance Eigen::VectorXd s = getSubstanceVector(substance, transform); // dim: 1xm int min_k = 0; double min_dist = 99999999; Eigen::VectorXd result(mean_vectors_.size()); for (unsigned int c = 0; c < mean_vectors_.size(); c++) { for (int k = 0; k < mean_vectors_[c].rows(); k++) { Eigen::VectorXd diff(s.rows()); for (unsigned int i = 0; i < s.rows(); i++) { diff(i) = s(i)-mean_vectors_[c](k, i); } double dist = diff.transpose()*sigma_*diff; if (dist < min_dist) { min_dist = dist; min_k = k; } } result(c) = labels_[min_k]; } return result; }
void Manipulator::update(const Eigen::VectorXd& q_msr, const Eigen::VectorXd& dq_msr) { if(q_msr.rows() != dof_) { std::stringstream msg; msg << "q_.rows() != dof" << std::endl << " q_.rows : " << q_msr.rows() << std::endl << " dof : " << dof_; throw ahl_utils::Exception("Manipulator::update", msg.str()); } if(dq_msr.rows() != dof_) { std::stringstream msg; msg << "dq_.rows() != dof" << std::endl << " dq_.rows : " << dq_msr.rows() << std::endl << " dof : " << dof_; throw ahl_utils::Exception("Manipulator::update", msg.str()); } q_ = q_msr; dq_ = dq_msr; this->computeForwardKinematics(); updated_joint_ = true; }
FitSurface::Domain::Domain(const Eigen::VectorXd& param0, const Eigen::VectorXd& param1) { if(param0.rows()!=param1.rows()) throw std::runtime_error("[FitSurface::Domain::Domain] Error, param vectors must be of same length."); double x_min(DBL_MAX), x_max(DBL_MIN); double y_min(DBL_MAX), y_max(DBL_MIN); for(Eigen::MatrixXd::Index i=0; i<param0.rows(); i++) { const double& p0 = param0(i); const double& p1 = param1(i); if(p0<x_min) x_min=p0; if(p0>x_max) x_max=p0; if(p1<y_min) y_min=p1; if(p1>y_max) y_max=p1; } x = x_min; y = y_min; width = x_max-x_min; height = y_max-y_min; }
gaussian_process::gaussian_process( const Eigen::MatrixXd& domains , const Eigen::VectorXd& targets , const gaussian_process::covariance& covariance , double self_covariance ) : domains_( domains ) , targets_( targets ) , covariance_( covariance ) , self_covariance_( self_covariance ) , offset_( targets.sum() / targets.rows() ) , K_( domains.rows(), domains.rows() ) { if( domains.rows() != targets.rows() ) { COMMA_THROW( comma::exception, "expected " << domains.rows() << " row(s) in targets, got " << targets.rows() << " row(s)" ); } targets_.array() -= offset_; // normalise //use m_K as Kxx + variance*I, then invert it //fill Kxx with values from covariance function //for elements r,c in upper triangle for( std::size_t r = 0; r < std::size_t( domains.rows() ); ++r ) { K_( r, r ) = self_covariance_; const Eigen::VectorXd& row = domains.row( r ); for( std::size_t c = r + 1; c < std::size_t( domains.rows() ); ++c ) { K_( c, r ) = K_( r, c ) = covariance_( row, domains.row( c ) ); } } L_.compute( K_ ); // invert Kxx + variance * I to become (by definition) B alpha_ = L_.solve( targets_ ); }
int TDynMinimalJerk::init(const std::vector<std::string>& parameters, RobotStatePtr robot_state, const Eigen::VectorXd& e_initial, const Eigen::VectorXd& e_final) { int size = parameters.size(); if (size != 3) { printHiqpWarning("TDynMinimalJerk requires 3 parameters, got " + std::to_string(size) + "! Initialization failed!"); return -1; } performance_measures_.resize(e_initial.rows()); e_dot_star_.resize(e_initial.rows()); time_start_ = robot_state->sampling_time_point_; total_duration_ = std::stod( parameters.at(1) ); gain_ = std::stod( parameters.at(2) ); f_ = 30 / total_duration_; e_initial_ = e_initial; e_final_ = e_final; e_diff_ = e_final - e_initial; return 0; }
void lpengine::ChooseLeavingVar(Eigen::VectorXd& delta_var, Eigen::VectorXd& var_hat, int* return_index, double* return_step_length) { Eigen::VectorXd tmp(var_hat.rows()); // choose smallest index (Bland's rule) for (int ii = 0; ii < var_hat.rows(); ii++) { if (var_hat[ii] == 0) { tmp[ii] = 0; *return_index = ii; return_step_length = 0; return; } else { tmp[ii] = delta_var[ii] / var_hat[ii]; } } std::ptrdiff_t max_index; tmp.maxCoeff(&max_index); *return_index = max_index; // calculate primal step size if (tmp[max_index] == 0) { *return_step_length = 0; } else { *return_step_length = 1 / tmp[max_index]; } }
ColMat<double, 3> FaceUnwrapper::interpolateFlatFace(const TopoDS_Face& face) { if (this->uv_nodes.size() == 0) throw(std::runtime_error("no uv-coordinates found, interpolating with nurbs is only possible if the Flattener was constructed with a nurbs.")); // extract xyz poles, knots, weights, degree const Handle(Geom_Surface) &_surface = BRep_Tool::Surface(face); const Handle(Geom_BSplineSurface) &_bspline = Handle(Geom_BSplineSurface)::DownCast(_surface); #if OCC_VERSION_HEX < 0x070000 TColStd_Array1OfReal _uknots(1, _bspline->NbUPoles() + _bspline->UDegree() + 1); TColStd_Array1OfReal _vknots(1, _bspline->NbVPoles() + _bspline->VDegree() + 1); _bspline->UKnotSequence(_uknots); _bspline->VKnotSequence(_vknots); #else const TColStd_Array1OfReal &_uknots = _bspline->UKnotSequence(); const TColStd_Array1OfReal &_vknots = _bspline->VKnotSequence(); #endif Eigen::VectorXd weights; weights.resize(_bspline->NbUPoles() * _bspline->NbVPoles()); long i = 0; for (long u=1; u <= _bspline->NbUPoles(); u++) { for (long v=1; v <= _bspline->NbVPoles(); v++) { weights[i] = _bspline->Weight(u, v); i++; } } Eigen::VectorXd u_knots; Eigen::VectorXd v_knots; u_knots.resize(_uknots.Length()); v_knots.resize(_vknots.Length()); for (long u=1; u <= _uknots.Length(); u++) { u_knots[u - 1] = _uknots.Value(u); } for (long v=1; v <= _vknots.Length(); v++) { v_knots[v - 1] = _vknots.Value(v); } nu = nurbs::NurbsBase2D(u_knots, v_knots, weights, _bspline->UDegree(), _bspline->VDegree()); A = nu.getInfluenceMatrix(this->uv_nodes); Eigen::LeastSquaresConjugateGradient<spMat > solver; solver.compute(A); ColMat<double, 2> ze_poles; ColMat<double, 3> flat_poles; ze_poles.resize(weights.rows(), 2); flat_poles.resize(weights.rows(), 3); flat_poles.setZero(); ze_poles = solver.solve(ze_nodes); flat_poles.col(0) << ze_poles.col(0); flat_poles.col(1) << ze_poles.col(1); return flat_poles; }
Eigen::MatrixXd Jacobian(double t, Eigen::VectorXd y){ Eigen::MatrixXd J(y.rows(),y.rows()); J(0,0) = 0.0; J(0,1) = 1.0; J(1,0) = -2000.0*y(0)*y(1)-y(0); J(1,1) = 1000.0*(1-y(0)*y(0)); return J; };
void FitOpenCurve::initSolver(const Eigen::VectorXd& params) { if(m_nurbs.CVCount() <= 0) throw std::runtime_error("[FitOpenCurve::initSolver] Error, curve not initialized (initCurve)."); if(params.rows()<2) throw std::runtime_error("[FitOpenCurve::initSolver] Error, insufficient parameter points (<2)."); int dim = m_nurbs.Dimension(); m_A = SparseMatrix( params.rows()*dim, m_nurbs.CVCount()*dim ); typedef Eigen::Triplet<double> Tri; std::vector<Tri> tripletList; tripletList.resize( dim * params.rows() * m_nurbs.Order() ); if(!m_quiet) printf("[FitOpenCurve::initSolver] entries: %lu rows: %lu cols: %d\n", dim * params.rows()* m_nurbs.Order(), dim * params.rows(), m_nurbs.CVCount()); double *N = new double[m_nurbs.m_order * m_nurbs.m_order]; int E; int ti(0); for(Eigen::MatrixXd::Index row=0; row<params.rows(); row++) { E = ON_NurbsSpanIndex (m_nurbs.m_order, m_nurbs.m_cv_count, m_nurbs.m_knot, params(row), 0, 0); ON_EvaluateNurbsBasis (m_nurbs.m_order, m_nurbs.m_knot + E, params(row), N); for (int i = 0; i < m_nurbs.Order(); i++) { tripletList[ti] = Tri( dim*row, dim*(E+i), N[i] ); if(dim>1) tripletList[ti+1] = Tri( dim*row+1, dim*(E+i)+1, N[i] ); if(dim>2) tripletList[ti+2] = Tri( dim*row+2, dim*(E+i)+2, N[i] ); ti+=dim; } // i } // row delete [] N; m_A.setFromTriplets(tripletList.begin(), tripletList.end()); if(m_solver==NULL) m_solver = new SPQR(); m_solver->compute(m_A); if(m_solver->info()!=Eigen::Success) throw std::runtime_error("[FitOpenCurve::initSolver] decomposition failed."); if(!m_quiet) printf("[FitOpenCurve::initSolver] decomposition done\n"); }
void utils_eigen::print_single_line(const Eigen::VectorXd &vec, int indents) { for(int k = 0; k<indents; k++) std::cout << "\t"; std::cout << "("; for(int i = 0; i<vec.rows()-1; i++) std::cout << vec(i,0) << ", "; std::cout << vec(vec.rows()-1,0) << ")" << std::endl; }
void ExperimentalTrajectory::addNewWaypoint(Eigen::VectorXd newWaypoint, double waypointTime) { std::cout << "Adding waypoint at: " << waypointTime << " seconds..." << std::endl; if ( (newWaypoint.rows() == nDoF) && (waypointTime>=0.0) && (waypointTime<=kernelCenters.maxCoeff()) ) { //Find out where the new T falls... for (int i=0; i<kernelCenters.size(); i++) { std::cout << "i = " << i << std::endl; if (kernelCenters(i)>waypointTime) { youngestWaypointIndex=i; std::cout << "youngestWaypointIndex" << youngestWaypointIndex << std::endl; i = kernelCenters.size(); } } Eigen::MatrixXd newWaypointsMat = Eigen::MatrixXd::Zero(nDoF, nWaypoints+1); newWaypointsMat.leftCols(youngestWaypointIndex) = waypoints.leftCols(youngestWaypointIndex); newWaypointsMat.col(youngestWaypointIndex) = newWaypoint; newWaypointsMat.rightCols(nWaypoints - youngestWaypointIndex) = waypoints.rightCols(nWaypoints - youngestWaypointIndex); waypoints.resize(nDoF, nWaypoints+1); waypoints = newWaypointsMat; Eigen::VectorXd durationVectorTmp = Eigen::VectorXd::Zero(nWaypoints); std::cout << "\npointToPointDurationVector\n " << pointToPointDurationVector << std::endl; durationVectorTmp.head(youngestWaypointIndex-1) = pointToPointDurationVector.head(youngestWaypointIndex-1); durationVectorTmp.row(youngestWaypointIndex-1) << waypointTime - kernelCenters(youngestWaypointIndex-1); durationVectorTmp.tail(nWaypoints - youngestWaypointIndex) = pointToPointDurationVector.tail(nWaypoints - youngestWaypointIndex); pointToPointDurationVector.resize(durationVectorTmp.size()); pointToPointDurationVector = durationVectorTmp; std::cout << "\npointToPointDurationVector\n " << pointToPointDurationVector << std::endl; reinitialize(); } else{ if (newWaypoint.rows() != nDoF){ std::cout << "[ERROR] (ExperimentalTrajectory::addNewWaypoint): New waypoint is not the right size. Should have dim = " <<nDoF << "x1." << std::endl; } if ((waypointTime<=0.0) || (waypointTime>=kernelCenters.maxCoeff())){ std::cout << "[ERROR] (ExperimentalTrajectory::addNewWaypoint): New waypoint time is out of time bounds. Should have fall between 0.0 and " << kernelCenters.maxCoeff() << " seconds." << std::endl; } } }
sva::RBInertiad sVectorToInertia(const Eigen::VectorXd& vec) { if(vec.rows() != 10) { std::ostringstream str; str << "Vector size mismatch: expected size is 10 gived is " << vec.rows(); throw std::out_of_range(str.str()); } return vectorToInertia(vec); }
EReturn AICOProblem::initDerived(tinyxml2::XMLHandle & handle) { EReturn ret_value = SUCCESS; tinyxml2::XMLElement* xmltmp; bool hastime = false; XML_CHECK("T"); XML_OK(getInt(*xmltmp, T)); if (T <= 2) { INDICATE_FAILURE ; return PAR_ERR; } xmltmp = handle.FirstChildElement("duration").ToElement(); if (xmltmp) { XML_OK(getDouble(*xmltmp, tau)); tau = tau / ((double) T); hastime = true; } if (hastime) { xmltmp = handle.FirstChildElement("tau").ToElement(); if (xmltmp) WARNING("Duration has already been specified, tau is ignored."); } else { XML_CHECK("tau"); XML_OK(getDouble(*xmltmp, tau)); } XML_CHECK("Qrate"); XML_OK(getDouble(*xmltmp, Q_rate)); XML_CHECK("Hrate"); XML_OK(getDouble(*xmltmp, H_rate)); XML_CHECK("Wrate"); XML_OK(getDouble(*xmltmp, W_rate)); { Eigen::VectorXd tmp; XML_CHECK("W"); XML_OK(getVector(*xmltmp, tmp)); W = Eigen::MatrixXd::Identity(tmp.rows(), tmp.rows()); W.diagonal() = tmp; } for (TaskDefinition_map::const_iterator it = task_defs_.begin(); it != task_defs_.end() and ok(ret_value); ++it) { if (it->second->type().compare(std::string("TaskSqrError")) == 0) ERROR("Task variable " << it->first << " is not an squared error!"); } // Set number of time steps return setTime(T); }
void Manipulator::init(uint32_t init_dof, const Eigen::VectorXd& init_q) { if(init_dof != init_q.rows()) { std::stringstream msg; msg << "dof != init_q.rows()" << std::endl << " dof : " << init_dof << std::endl << " init_q.rows() : " << init_q.rows(); throw ahl_utils::Exception("Manipulator::init", msg.str()); } dof_ = init_dof; // Resize vectors and matrices q_ = Eigen::VectorXd::Zero(dof_); dq_ = Eigen::VectorXd::Zero(dof_); T_.resize(link_.size()); for(uint32_t i = 0; i < T_.size(); ++i) { T_[i] = link_[i]->T_org; } T_abs_.resize(dof_ + 1); for(uint32_t i = 0; i < T_abs_.size(); ++i) { T_abs_[i] = Eigen::Matrix4d::Identity(); } C_abs_.resize(dof_ + 1); for(uint32_t i = 0; i < C_abs_.size(); ++i) { C_abs_[i] = Eigen::Matrix4d::Identity(); } Pin_.resize(dof_ + 1); for(uint32_t i = 0; i < Pin_.size(); ++i) { Pin_[i] = Eigen::Vector3d::Zero(); } q_ = init_q; differentiator_ = std::make_shared<ahl_filter::PseudoDifferentiator>(update_rate_, cutoff_frequency_); differentiator_->init(q_, dq_); this->computeForwardKinematics(); J_.resize(link_.size()); M_.resize(dof_, dof_); M_inv_.resize(dof_, dof_); for(uint32_t i = 0; i < link_.size(); ++i) { name_to_idx_[link_[i]->name] = i; } }
void OMPLRNStateSpace::OMPLToExoticaState(const ompl::base::State *state, Eigen::VectorXd &q) const { if (!state) { throw_pretty("Invalid state!"); } if (q.rows() != (int) getDimension()) q.resize((int) getDimension()); memcpy(q.data(), state->as<OMPLRNStateSpace::StateType>()->getRNSpace().values, sizeof(double) * q.rows()); }
VectorXd CGaussianLikelihood::get_second_derivative(CRegressionLabels* labels, TParameter* param, CSGObject* obj, Eigen::VectorXd function) { if (strcmp(param->m_name, "sigma") || obj != this) { VectorXd result(function.rows()); result(0) = CMath::INFTY; return result; } return 2*VectorXd::Ones(function.rows())/(m_sigma*m_sigma); }
int main(int argc, char** argv) { ros::init(argc, argv, "parser_test"); ros::NodeHandle nh; try { std::string name = "lwr"; RobotPtr robot = std::make_shared<Robot>(name); ParserPtr parser = std::make_shared<Parser>(); std::string path = "/home/daichi/Work/catkin_ws/src/ahl_ros_pkg/ahl_robot/ahl_robot/yaml/lwr.yaml"; parser->load(path, robot); ros::MultiThreadedSpinner spinner; TfPublisherPtr tf_publisher = std::make_shared<TfPublisher>(); const std::string mnp_name = "mnp"; unsigned long cnt = 0; ros::Rate r(10.0); while(ros::ok()) { Eigen::VectorXd q = Eigen::VectorXd::Zero(robot->getDOF(mnp_name)); ManipulatorPtr mnp = robot->getManipulator(mnp_name); double goal = sin(2.0 * M_PI * 0.2 * cnt * 0.1); ++cnt; for(uint32_t i = 0; i < q.rows(); ++i) { q.coeffRef(i) = M_PI / 4.0 * goal; } q = Eigen::VectorXd::Constant(q.rows(), M_PI / 4.0); robot->update(q); robot->computeJacobian(mnp_name); robot->computeMassMatrix(mnp_name); M = robot->getMassMatrix(mnp_name); J = robot->getJacobian(mnp_name, "gripper"); calc(); tf_publisher->publish(robot, false); r.sleep(); } } catch(ahl_utils::Exception& e) { ROS_ERROR_STREAM(e.what()); } return 0; }
Eigen::VectorXd CochlearFilterbank::process_channel(const Eigen::VectorXd &input, int n, int ch) { Eigen::VectorXd y1(Eigen::VectorXd::Zero(input.rows())); Eigen::VectorXd y2(Eigen::VectorXd::Zero(input.rows())); Eigen::VectorXd y3(Eigen::VectorXd::Zero(input.rows())); Eigen::VectorXd y4(Eigen::VectorXd::Zero(input.rows())); y1 = filters[ch][0].process(input, n); y2 = filters[ch][1].process(y1, n); y3 = filters[ch][2].process(y2, n); y4 = filters[ch][3].process(y3, n); return y4; }
/** * @brief hqp_wrapper::addStage Adds stage (Jq <=> B) * @param levelName String to be used as ID * @param J * @param B * @param bound Type of bound (<,>,=) * @return */ int hqp_wrapper::addStage(std::string levelName,Eigen::MatrixXd J, Eigen::VectorXd B, soth::Bound::bound_t bound){ soth::VectorBound vB; std::cout <<"B.rows:"<<B.rows()<<"\n"; vB.resize(B.rows()); for(int i=0;i<B.rows();i++){ vB[i] = soth::Bound(B[i],bound); } this->J.push_back(J); this->B.push_back(vB); this->Btyp.push_back(bound); leveNameMap.insert(std::make_pair(levelName,this->B.size()-1)); return 0; }
void OMPLRNStateSpace::ExoticaToOMPLState(const Eigen::VectorXd &q, ompl::base::State *state) const { if (!state) { throw_pretty("Invalid state!"); } if (q.rows() != (int) getDimension()) { throw_pretty( "State vector ("<<q.rows()<<") and internal state ("<<(int)getDimension()<<") dimension disagree"); } memcpy(state->as<OMPLRNStateSpace::StateType>()->getRNSpace().values, q.data(), sizeof(double) * q.rows()); }
void deleteVectorBlock( Eigen::VectorXd &matrix, const int block_start_index, const int block_length ){ assert( 1==matrix.cols() ); assert( matrix.rows()>=block_start_index+block_length ); int size = matrix.rows(); /* R D M */ matrix.block( block_start_index, 0, size-block_start_index-block_length, 1 ) = matrix.block( block_start_index+block_length, 0, size-block_start_index-block_length, 1 ); matrix.conservativeResize( size-block_length, 1 ); }
void print_eigen(const Eigen::VectorXd& command) { std::string cmd_str = " "; for(size_t i=0; i<command.rows(); ++i) cmd_str += boost::lexical_cast<std::string>(command[i]) + " "; ROS_INFO("Command: (%s)", cmd_str.c_str()); }
/* Function: testACASolverConv1DUniformPts * --------------------------------------- * This function creates a convergence plot of solver relative error vs ACA tolerance for a dense self interaction matrix. * The test dense matrix, is an interaction matrix arising from the self interaction of uniform points on a 1D interval. * intervalMin : Lower bound of the 1D interval. * intervalMax : Upper bound of the 1D interval. * numPts : Number of interacting points (matrix size). * diagValue : The diagonal entry value of the dense matrix. * exactSoln : The test right hand side of the linear system. * outputFileName : Path of the output file. * kernel : Pointer to the kernel function. * solverType : Type of HODLR solver. Default is recLU. */ void testACASolverConv1DUnifromPts(const double intervalMin,const double intervalMax, const int numPts, const double diagValue, Eigen::VectorXd exactSoln, std::string outputFileName, double (*kernel)(const double r),std::string solverType){ assert(intervalMax > intervalMin); assert(numPts > 0); assert(exactSoln.rows() == numPts); int minTol = -5; int maxTol = -10; int sizeThreshold = 30; Eigen::MatrixXd denseMatrix = makeMatrix1DUniformPts (intervalMin, intervalMax, intervalMin, intervalMax, numPts, numPts, diagValue, kernel); Eigen::VectorXd RHS = denseMatrix * exactSoln; HODLR_Matrix denseHODLR(denseMatrix, sizeThreshold); std::ofstream outputFile; outputFile.open(outputFileName.c_str()); for (int i = minTol; i >= maxTol; i--){ double tol = pow(10,i); denseHODLR.set_LRTolerance(tol); Eigen::VectorXd solverSoln; if (solverType == "recLU") solverSoln = denseHODLR.recLU_Solve(RHS); if (solverType == "extendedSp") solverSoln = denseHODLR.extendedSp_Solve(RHS); Eigen::VectorXd residual = solverSoln-exactSoln; double relError = residual.norm()/exactSoln.norm(); outputFile<<tol<<" "<<relError<<std::endl; } outputFile.close(); }
void Camera::predict( double dt, Eigen::VectorXd &new_x, Eigen::MatrixXd &jacobi, Eigen::MatrixXd &noise ){ assert( new_x.rows()>=this->index_offset+Camera::DIM ); assert( jacobi.cols()==jacobi.rows() ); assert( jacobi.cols()>=this->index_offset+Camera::DIM ); CameraPredictFunc fv(dt); double calc_jacobian[Camera::DIM*Camera::DIM]; double predicted_state[Camera::DIM]; double old_x[Camera::DIM]; for(int i = 0; i<Camera::DIM; i++){ old_x[i] = this->map->state( this->index_offset+i ); } double *parameters[] = { old_x }; double *jacobians[] = { calc_jacobian }; AutoDiff<CameraPredictFunc, double, Camera::DIM>::Differentiate( fv, parameters, Camera::DIM, predicted_state, jacobians); for(int i = 0; i<Camera::DIM; i++){ new_x( this->index_offset+i ) = predicted_state[i]; for(int j = 0; j<Camera::DIM; j++){ jacobi( this->index_offset+i, this->index_offset+j ) = calc_jacobian[ j+i*Camera::DIM ]; } } for(int i=0; i<4; i++){ noise( this->index_offset+i, this->index_offset+i ) = 0.001; } for(int i=4; i<7; i++){ noise( this->index_offset+i, this->index_offset+i ) = 0.1; } }
void JointControl::computeGeneralizedForce(Eigen::VectorXd& tau) { tau = Eigen::VectorXd::Zero(mnp_->getDOF()); Eigen::VectorXd error = qd_ - mnp_->q(); Eigen::MatrixXd Kpv = param_->getKpJoint().block(0, 0, mnp_->getDOF(), mnp_->getDOF()); for(uint32_t i = 0; i < Kpv.rows(); ++i) { Kpv.coeffRef(i, i) /= param_->getKvJoint().block(0, 0, mnp_->getDOF(), mnp_->getDOF()).coeff(i, i); } Eigen::VectorXd dqd = Kpv * error; //const double dq_max = 25.0; for(uint32_t i = 0; i < dqd.rows(); ++i) { if(dqd[i] < -param_->getDqMax()) { dqd[i] = -param_->getDqMax(); } else if(dqd[i] > param_->getDqMax()) { dqd[i] = param_->getDqMax(); } } Eigen::VectorXd tau_unit = -param_->getKvJoint().block(0, 0, mnp_->getDOF(), mnp_->getDOF()) * (mnp_->dq() - dqd); tau = tau_ = mnp_->getMassMatrix() * tau_unit; }
void Polytope::dumpJSONcore( std::ostream &out ) const { int i, j; out << "\"H\": ["; for (i = 0; i < H.rows(); i++) { if (i > 0) { out << ", "; } out << "["; for (j = 0; j < H.cols(); j++) { if (j > 0) out << ", "; out << H(i,j); } out << "]"; } out << "], \"K\": ["; for (i = 0; i < K.rows(); i++) { if (i > 0) { out << ", "; } out << K(i); } out << "]"; }
TEST(decimate,hemisphere) { // Load a hemisphere centered at the origin. For each original vertex compute // its "perfect normal" (i.e., its position treated as unit vectors). // Decimate the model and using the birth indices of the output vertices grab // their original "perfect normals" and compare them to their current // positions treated as unit vectors. If vertices have not moved much, then // these should be similar (mostly this is checking if the birth indices are // sane). Eigen::MatrixXd V,U; Eigen::MatrixXi F,G; Eigen::VectorXi J,I; // Load example mesh: GetParam() will be name of mesh file test_common::load_mesh("hemisphere.obj", V, F); // Perfect normals from positions Eigen::MatrixXd NV = V.rowwise().normalized(); // Remove half of the faces igl::decimate(V,F,F.rows()/2,U,G,J,I); // Expect that all normals still point in same direction as original Eigen::MatrixXd NU = U.rowwise().normalized(); Eigen::MatrixXd NVI; igl::slice(NV,I,1,NVI); ASSERT_EQ(NVI.rows(),NU.rows()); ASSERT_EQ(NVI.cols(),NU.cols()); // Dot product Eigen::VectorXd D = (NU.array()*NVI.array()).rowwise().sum(); Eigen::VectorXd O = Eigen::VectorXd::Ones(D.rows()); // 0.2 chosen to succeed on 256 face hemisphere.obj reduced to 128 faces test_common::assert_near(D,O,0.02); }
Eigen::VectorXd FitModel::predict(const vector<double> & substance, bool transform) { if (training_result_.cols() == 0) { throw Exception::InconsistentUsage(__FILE__, __LINE__, "Model must be trained before it can predict the activitiy of substances!"); } Eigen::VectorXd v = getSubstanceVector(substance, transform); Eigen::VectorXd res(Y_.cols()); String var=""; // replace all x-values for the current substance for (unsigned int j = 0; j < v.rows(); j++) { var = var+"x"+String(j)+"="+String(v(j))+";"; } //calculated all activities for given substance for (int c = 0; c < Y_.cols(); c++) { String coeff=""; // get optimized coefficients for (int m = 0; m < training_result_.rows(); m++) { coeff = coeff+"b"+String(m)+"="+String(training_result_(m, c))+";"; } ParsedFunction<float> f = coeff+var+allEquations_[c]; res(c) = f(0); } if (transform && y_transformations_.cols() != 0) { backTransformPrediction(res); } return res; }
Eigen::VectorXd TrajectoryThread::varianceToWeights(Eigen::VectorXd& desiredVariance, const double beta) { desiredVariance /= maximumVariance; desiredVariance = desiredVariance.array().min(varianceThresh); //limit desiredVariance to 0.99 maximum Eigen::VectorXd desiredWeights = (Eigen::VectorXd::Ones(desiredVariance.rows()) - desiredVariance) / beta; return desiredWeights; }
float64_t CGaussianLikelihood::get_log_probability_f(CRegressionLabels* labels, Eigen::VectorXd function) { VectorXd result(function.rows()); for (index_t i = 0; i < function.rows(); i++) result[i] = labels->get_labels()[i] - function[i]; result = result.cwiseProduct(result); result /= -2*m_sigma*m_sigma; for (index_t i = 0; i < function.rows(); i++) result[i] -= log(2*CMath::PI*m_sigma*m_sigma)/2.0; return result.sum(); }