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();
}
