Example #1
0
double weight_gaussian_predictive_rev(Gaussian &g1, Gaussian &g2)
{
    int dim = g1.dim;
    double energy1 = 0.;
            Eigen::MatrixXd cov = g1.predictive_covariance + g1.predictive_covariance;
            Eigen::VectorXd mean = g1.predictive_mean - g1.predictive_mean;
            Eigen::MatrixXd invij = cov.inverse();
            double a = mean.transpose()*invij*mean;
            double gauss = 1./sqrt(pow(2*pi,dim)*cov.determinant())*exp(-0.5*a);
            energy1 += gauss;

    double energy2 = 0.;

            cov = g1.predictive_covariance + g2.predictive_covariance;
            mean = g1.predictive_mean - g2.predictive_mean;
            invij = cov.inverse();
            a = mean.transpose()*invij*mean;
            gauss = 1./sqrt(pow(2*pi,dim)*cov.determinant())*exp(-0.5*a);
            energy2 += gauss;

    double energy3 = 0.;
            cov = g2.predictive_covariance + g2.predictive_covariance;
            mean = g2.predictive_mean - g2.predictive_mean;
            invij = cov.inverse();
            a = mean.transpose()*invij*mean;
            gauss = 1./sqrt(pow(2*pi,dim)*cov.determinant())*exp(-0.5*a);
            energy3 += gauss;
//    cout<<(maxDist - (energy1 - 2*energy2 + energy3))/maxDist<<endl;
    return energy1 - 2*energy2 + energy3;

}
   bool CartTrajPlanner::jspace_trivial_path_planner(Eigen::VectorXd q_start,Eigen::VectorXd q_end,std::vector<Eigen::VectorXd> &optimal_path) {
       Eigen::VectorXd qx_start(NJNTS),qx_end(NJNTS);// need to convert to this type

       cout<<"jspace_trivial_path_planner: "<<endl;
       cout<<"q_start: "<<q_start.transpose()<<endl;
       cout<<"q_end: "<<q_end.transpose()<<endl;
       optimal_path.clear();
       optimal_path.push_back(q_start);
       optimal_path.push_back(q_end);
       return true;
   }
Example #3
0
double weight_l2_rev(PCObject &o1, PCObject &o2)
{
    double last = pcl::getTime ();
    // reference :
    // Robust Point Set Registration Using Gaussian Mixture Models
    // Bing Jina, and Baba C. Vemuri
    // IEEE Transactions on Pattern Analysis and Machine Intelligence 2010
    int n = o1.gmm.size();
    int m = o2.gmm.size();

    double energy1 = 0.;
    for(int i=0;i<n;i++){
        for(int j=0;j<n;j++){
            int dim = o1.gmm.at(i).dim;
            Eigen::MatrixXd cov = o1.gmm.at(i).covariance + o1.gmm.at(j).covariance;
            Eigen::VectorXd mean = o1.gmm.at(i).mean - o1.gmm.at(j).mean;
            Eigen::MatrixXd invij = cov.inverse();
            double a = mean.transpose()*invij*mean;
            double gauss = 1./sqrt(pow(2*pi,dim)*cov.determinant())*exp(-0.5*a);
            energy1 += o1.gmm.at(i).weight*o1.gmm.at(j).weight*gauss;
        }
    }
    double energy2 = 0.;
    for(int i=0;i<n;i++){
        for(int j=0;j<m;j++){
            int dim = o1.gmm.at(i).dim;
            Eigen::MatrixXd cov = o1.gmm.at(i).covariance + o2.gmm.at(j).covariance;
            Eigen::VectorXd mean = o1.gmm.at(i).mean - o2.gmm.at(j).mean;
            Eigen::MatrixXd invij = cov.inverse();
            double a = mean.transpose()*invij*mean;
            double gauss = 1./sqrt(pow(2*pi,dim)*cov.determinant())*exp(-0.5*a);
            energy2 += o1.gmm.at(i).weight*o2.gmm.at(j).weight*gauss;
        }
    }
    double energy3 = 0.;
    for(int i=0;i<m;i++){
        for(int j=0;j<m;j++){
            int dim = o2.gmm.at(i).dim;
            Eigen::MatrixXd cov = o2.gmm.at(i).covariance + o2.gmm.at(j).covariance;
            Eigen::VectorXd mean = o2.gmm.at(i).mean - o2.gmm.at(j).mean;
            Eigen::MatrixXd invij = cov.inverse();
            double a = mean.transpose()*invij*mean;
            double gauss = 1./sqrt(pow(2*pi,dim)*cov.determinant())*exp(-0.5*a);
            energy3 += o2.gmm.at(i).weight*o2.gmm.at(j).weight*gauss;
        }
    }
    double now = pcl::getTime ();
//    cout << "l2-distance time " << now-last << " second" << endl;
    //    cout<<"l2distance"<<energy1 - 2*energy2 + energy3<<endl;

    return (energy1 - 2*energy2 + energy3);
}
bool ArmMotionInterface::plan_path_to_predefined_pre_pose(void) {
    cout << "called plan_path_to_predefined_pre_pose" << endl;
    cout << "setting q_start and q_end: " << endl;
    q_vec_end_resp_ = q_pre_pose_;
    cout << "q pre-pose goal: " << q_vec_end_resp_.transpose() << endl;
    q_vec_start_resp_ = q_vec_right_arm_;
    cout << "q start = current arm state: " << q_vec_start_resp_.transpose() << endl;
    //plan a path: from q-start to q-goal...what to do with wrist??
    cout << "NOT DONE YET" << endl;
    path_is_valid_ = false;
    return path_is_valid_;

}
bool SufficientZChange::initialize(Eigen::VectorXd& HP) {

	if (_zHistory.size() >= 2) {

	  auto it = _zHistory.begin();
	  const ObservationDescriptor *first = &(it->second);

		while (++it != _zHistory.end()) {
		_zChange = max(_zChange, (it->second.z - first->z).norm());
		}
	}

	if (_zChange > _minZChange && _zHistory.size() >= 5) {
		const ObservationDescriptor &first = _zHistory.begin()->second;

		Eigen::Vector3d z0;
		z0 << first.z(0), first.z(1), 1.0;

		HP << _K.inverse() * z0;
		HP(2) = 1.0 / _initialDepth; // 1/d distance of the plane parallel to the image plane on which features are initialized.

#		ifdef DEBUG_PRINT_VISION_INFO_MESSAGES
		cerr << fixed << "[SufficientZChange] HP initialization: " << HP.transpose() << endl;
#		endif

		return true;
	}

	return false;
}
Example #6
0
/* ********************************************************************************************* */
bool getArmConfFull (Part* part, size_t faceId, const Eigen::VectorXd& tL, const Eigen::VectorXd& c, 
		const Eigen::VectorXd& qb, bool right, Eigen::VectorXd& qa) {

	// Compute the location of the point
	Eigen::Matrix4d Tl; 
	createTransform(tL, Tl);
	cout << "c: " << c.transpose() << endl;

	// Get the normal of the face
	// Eigen::Vector3d normal2 = Tl.topLeftCorner<3,3>() * part->getPlane(faceId).topLeftCorner<3,1>();
	// Eigen::Vector3d normal = part->getVertexW(faceId, 0) - part->getVertexW(faceId, 1);
	Eigen::Vector3d normal = part->getPlane(faceId).topLeftCorner<3,1>();
	normal = (Tl.topLeftCorner<3,3>() * normal).normalized();
	Eigen::Matrix3d rotM;
	rotM.block<3,1>(0,2) = -normal;
 	Eigen::Vector3d col2 = Eigen::Vector3d::Random(3);
	rotM.block<3,1>(0,1) = (col2 - col2.dot(normal) * normal).normalized();
	// rotM.block<3,1>(0,1) = normal2;
	rotM.block<3,1>(0,0) = rotM.block<3,1>(0,1).cross(rotM.block<3,1>(0,2));

	// Create the goal matrix
	Matrix4d Twee = Matrix4d::Identity();
	Twee.topRightCorner<3,1>() = c;// - normal * 0.05;
	Twee.topLeftCorner<3,3>() = rotM;

	// Make the call
	Vector7d bla;
	bool result = singleArmIK(qb, Twee, right, bla, true);
	qa = Vector7d(bla);
	if(!result) cout << "can not reach: " << qa(0) << endl;
	return result;
}
Example #7
0
    Eigen::VectorXd operator()(const F& obj,  Eigen::VectorXd x) const {
        Eigen::VectorXd g, d;
        Eigen::MatrixXd G;
        LineSearch lineSearch;
        obj(x, g);
        obj(x, G);

        DEBUG_PRINT(x);
        DEBUG_PRINT(g);
        DEBUG_PRINT(G);
        DEBUG_PRINT(G.ldlt().solve(g));
        while(g.norm() > 10e-10) {
#if DEBUG
            std::cout << std::string(20, '-') << std::endl;
#endif
            d = G.ldlt().solve(-g);
            DEBUG_PRINT(G.ldlt().solve(-g).transpose());
            DEBUG_PRINT(G.ldlt().vectorD());

            Real a = 1.;
            a = lineSearch(obj, x, d, a);

            DEBUG_PRINT(a);
            DEBUG_PRINT(obj(x));
            x = x +  a * d;
            obj(x, g);
            obj(x, G);
            DEBUG_PRINT(x.transpose());
            DEBUG_PRINT(g.transpose());
            //	DEBUG_PRINT(G);
        }
        return x;
    }
Example #8
0
void Model::construct_preference_and_covariance(
    Eigen::VectorXd &Eta, Eigen::MatrixXd &Omega, const Eigen::MatrixXd &C,
    const Eigen::MatrixXd &M, const Eigen::VectorXd &W,
    const Eigen::MatrixXd &S, const Parameter &parameter) {

  Eigen::VectorXd Mu = C * M * W;
  Eta = Mu;

  Eigen::MatrixXd Psi = W.asDiagonal();
  Psi -= (W * W.transpose());

  Eigen::MatrixXd I(n_alternatives, n_alternatives);
  I.setIdentity();
  Eigen::MatrixXd Phi = C * M * Psi * M.transpose() * C.transpose() +
                        parameter.sig2 * C * I * C.transpose();

  Omega = Phi;
  double rt;
  Eigen::MatrixXd Si = I;

  for (int i = 2; i <= parameter.stopping_time; i++) {
    rt = 1.0 / (1 + exp(i - 202.0) / 25); // [Takao] Not sure what this is.
    Si = S * Si;
    Eta += rt * Si * Mu;
    Omega += pow(rt, 2) * Si * Phi * Si.transpose();
  }
}
void actionStateCallback(const std_msgs::String::ConstPtr& msg)
{
	ROS_WARN("I heard action: [%s]. Doing SHIFT...", msg->data.c_str());
	if (msg->data.c_str()=="reach"){
		shift_ee_ft = est_ee_ft;
		ROS_WARN("Shiftiing!!");
		ROS_WARN_STREAM("Shift: "<<shift_ee_ft.transpose());
	} 
}
Example #10
0
        /** Compute the potential of a node according to the weights and its
         * features.
         * \param features: features of the node.
         * \return potentials of that node.
         */
        Eigen::VectorXd computePotentials( Eigen::VectorXd &features )
        {
            // Check that features are a column vector, and transpose it  otherwise
            if ( features.cols() > 1 )
                        features = features.transpose();

            assert ( features.rows() == m_nFeatures );

            return (*m_computePotentialsFunction)(m_weights,features);
        }
void CLMBlockModel::optimise()
{
    this->run(); //Full recompute of all the 0-states, which will never be recomputed again.
	CLMFullModel fullModelFn(this);
    CLevMar LM(fullModelFn, true, 1e-7);
    Eigen::VectorXd params = fullModelFn.init();
    cout << params.transpose() << endl;
    LM.minimise(params, 5);
    cout << "Optimisation complete" << endl;
}
Example #12
0
 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;
 }
Example #13
0
int main(int argc, char **argv) {
    ros::init(argc, argv, "rrbot_fk_test");
    ros::NodeHandle nh;

    Eigen::Affine3d affine_flange;

    Rrbot_fwd_solver rrbot_fwd_solver;
    //Rrbot_IK_solver rrbot_ik_solver;    
    
    Eigen::Vector3d flange_origin_wrt_world, perturbed_flange_origin, dp, Jdq_trans;
    Eigen::VectorXd Jdq;

    double q_elbow, q_shoulder; 
    double q_elbow_perturbed,q_shoulder_perturbed;
    double delta_q = 0.000001;
    Eigen::MatrixXd J;
    double diff;
    Eigen::MatrixXd q_vec(2,1),dq_vec(2,1);
    q_vec<<0,0;
    dq_vec<<delta_q,delta_q;

    while (ros::ok()) {
        cout<<endl<<endl;

        q_vec(0) = q_shoulder;
        q_vec(1) = q_elbow;
        ROS_INFO("angs: %f, %f", q_elbow, q_shoulder);

        affine_flange = rrbot_fwd_solver.fwd_kin_flange_wrt_world_solve(q_vec);
        flange_origin_wrt_world = affine_flange.translation();
        cout << "FK: flange origin: " << flange_origin_wrt_world.transpose() << endl;
        //perturb the joint angles and recompute fwd kin:
        affine_flange = rrbot_fwd_solver.fwd_kin_flange_wrt_world_solve(q_vec+dq_vec);
        perturbed_flange_origin = affine_flange.translation();
        dp = perturbed_flange_origin - flange_origin_wrt_world;
        cout<<"dp = "<<dp.transpose()<<endl;
        J = rrbot_fwd_solver.Jacobian(q_vec);
        cout<<"J: "<<endl;
        cout<<J<<endl;
        cout<<"dq = "<<dq_vec.transpose()<<endl;
        Jdq = J*dq_vec;
        cout<<"Jdq = "<<Jdq.transpose()<<endl;
        
        for (int i=0;i<3;i++) Jdq_trans(i) = Jdq(i);

        cout<<"Jdq_trans = "<<Jdq_trans.transpose()<<endl;
        diff = (Jdq_trans - dp).norm();
        cout<<"diff = "<<diff<<endl;
        
        ros::spinOnce();
        ros::Duration(1.0).sleep();
        q_elbow = q_lower_limits[0] + (q_upper_limits[0]-q_lower_limits[0])*(rand() % 100)/100.0;  
        q_shoulder = q_lower_limits[0] + (q_upper_limits[1]-q_lower_limits[1])*(rand() % 100)/100.0;        
    }
}
Example #14
0
    void CovarianceEstimator::update(uint i, const Eigen::VectorXd &sample)
    {
      // 10 is theoretically optimal
      double n = (double)lengths_[i] + 10 * sample.size();

      a_[i] = a_[i] * (n / (n + 1)) + (sample * sample.transpose()) / (n + 1);
      u_[i] = u_[i] * (n / (n + 1)) + sample / (n + 1);

      covs_[i] = a_[i] - (u_[i] * u_[i].transpose());// / (n + 1);

      lengths_[i]++;
    }
Example #15
0
  double multivariateGaussianDensity(const Eigen::VectorXd& mean,
                                         const Eigen::MatrixXd& cov,
                                         const Eigen::VectorXd& z)
  {
    Eigen::VectorXd diff = mean - z;

    Eigen::VectorXd exponent = -0.5 * (diff.transpose() * cov.inverse() * diff);

    return pow(2 * M_PI, (double) z.size() / -2.0) * pow(cov.determinant(), -0.5) *
    exp(exponent(0));

  }
Example #16
0
double weight_l2_gauss(PCObject &o1, PCObject &o2)
{
    // l2 distance
    Eigen::MatrixXd covsum = o1.gaussian.covariance+o2.gaussian.covariance;
    Eigen::VectorXd meandiff = o1.gaussian.mean - o2.gaussian.mean;
    Eigen::MatrixXd inv = covsum.inverse();
    double det = covsum.determinant();
    double a = meandiff.transpose()*inv*meandiff;
    double l2 = 2.-2.*(1./sqrt(pow(2*pi,3)*det)) * exp(-0.5*a);
    if(l2 < 0) l2 = 0.;
    return l2;
}
Example #17
0
double weight_unsymkl_gauss(PCObject &o1, PCObject &o2)
{
    int dim = o1.gaussian.dim;
    Eigen::MatrixXd multicov = Eigen::MatrixXd(3,3);
    multicov = o2.gaussian.cov_inverse * o1.gaussian.covariance;
    Eigen::VectorXd mean = o2.gaussian.mean-o1.gaussian.mean;
    double unsymkl_12 = (multicov.trace()
                         + mean.transpose()*o2.gaussian.cov_inverse*mean
                         + log(o1.gaussian.cov_determinant/o2.gaussian.cov_determinant)-dim) / 2.;
//    cout<<"kl: "<<unsymkl_12<<endl;
    return unsymkl_12;
}
bool ArmMotionInterface::unpack_qend(void) {
    int njoints = request_.q_vec_end.size();
    //cout<<"size of request q_end: "<< njoints<<endl;
    if (njoints != 7) {
        ROS_WARN("received bad joint-space goal: njoints = %d", njoints);
        return false;
    }
    for (int i = 0; i < 7; i++) {
        q_vec_end_rqst_[i] = request_.q_vec_end[i];
    }
    cout << "unpacked q_vec_goal from request: " << q_vec_end_rqst_.transpose() << endl;
    return true;
}
Example #19
0
      Eigen::VectorXd predict(const Eigen::VectorXd& current, const Eigen::VectorXd& target){
	
	Eigen::VectorXd error=target-current;
	_D = error-_P;
	_P = error;
	_I += error;
	Eigen::VectorXd action;
	
	action = _mP*_P + _mI*_I + _mD*_D;
	
	std::cout<<"error "<< error.transpose() <<" action: "<<action.transpose()<<std::endl;
	return action;
      }
Eigen::VectorXd TargetTrackingController::getControl(const EKF *ekf, const MultiAgentMotionModel *motionModel, const std::vector<const SensorModel*> &sensorModel, double *f) const {
  Evaluator evaluator(ekf, motionModel, sensorModel, params);

  Eigen::VectorXd p = Eigen::VectorXd::Zero(motionModel->getControlDim());
  Eigen::VectorXd lowerBound = Eigen::VectorXd::Constant(motionModel->getControlDim(), params("multi_rotor_control/controlMin").toDouble());
  Eigen::VectorXd upperBound = Eigen::VectorXd::Constant(motionModel->getControlDim(), params("multi_rotor_control/controlMax").toDouble());

  nlopt_opt opt = nlopt_create(NLOPT_LN_COBYLA, p.size());
//  nlopt_opt opt = nlopt_create(NLOPT_LN_BOBYQA, p.size());
//  nlopt_opt opt = nlopt_create(NLOPT_LN_NEWUOA_BOUND, p.size());
//  nlopt_opt opt = nlopt_create(NLOPT_LN_PRAXIS, p.size());
//  nlopt_opt opt = nlopt_create(NLOPT_LN_NELDERMEAD, p.size());
//  nlopt_opt opt = nlopt_create(NLOPT_LN_SBPLX, p.size());
//  nlopt_opt opt = nlopt_create(NLOPT_GN_ORIG_DIRECT, p.size()); // failed
//  nlopt_opt opt = nlopt_create(NLOPT_GN_ORIG_DIRECT_L, p.size()); // very good: p    0.0118546 -6.27225e-05  6.27225e-05 -2.09075e-05  2.09075e-05 -8.51788e-06 -2.09075e-05           10
//  nlopt_opt opt = nlopt_create(NLOPT_GN_ISRES, p.size()); // rather bad
//  nlopt_opt opt = nlopt_create(NLOPT_GN_CRS2_LM, p.size());
//  nlopt_opt opt = nlopt_create(NLOPT_LD_MMA, p.size());
//  nlopt_opt opt = nlopt_create(NLOPT_LD_CCSAQ, p.size());
//  nlopt_opt opt = nlopt_create(NLOPT_LD_SLSQP, p.size());
//  nlopt_opt opt = nlopt_create(NLOPT_LD_LBFGS, p.size());
//  nlopt_opt opt = nlopt_create(NLOPT_LD_TNEWTON_PRECOND, p.size()); // bad
//  nlopt_opt opt = nlopt_create(NLOPT_LD_TNEWTON_PRECOND_RESTART, p.size()); // bad
//  nlopt_opt opt = nlopt_create(NLOPT_LD_VAR2, p.size());

  nlopt_set_min_objective(opt, f_evaluate, &evaluator);
  nlopt_set_lower_bounds(opt, lowerBound.data());
  nlopt_set_upper_bounds(opt, upperBound.data());
  nlopt_set_ftol_abs(opt, 1E-6);
  nlopt_set_xtol_rel(opt, 1E-3);
  nlopt_set_maxeval(opt, 1E8);
  nlopt_set_maxtime(opt, 7200);
  double pa[p.size()];
  memcpy(pa, p.data(), p.size()*sizeof(double));
  double cost = 0;
//  std::string tmp; std::cerr << "Press enter to start optimization\n"; std::getline(std::cin, tmp);
  nlopt_result ret = nlopt_optimize(opt, pa, &cost);
  Eigen::VectorXd p_res = Eigen::Map<Eigen::VectorXd>(pa, p.size());
  if (f)
    *f = cost;

  std::cerr << "\nInitial guess:\n";
  std::cerr << "  p " << p.transpose() << "\n";
  std::cerr << "  value " << evaluator.evaluate(p) << "\n";

  std::cerr << "Optimization result (return code " << ret << "):\n";
  std::cerr << "  p " << p_res.transpose() << "\n";
  std::cerr << "  value " << evaluator.evaluate(p_res) << "\n";
  nlopt_destroy(opt);
  return p_res;
}
void jointStatesCb(const sensor_msgs::JointState& js_msg) {
    //joint_states_ = js_msg; // does joint-name mapping only once
    if (g_arm_joint_indices.size()<1) {
        int njnts = js_msg.position.size();
        ROS_INFO("finding joint mappings for %d jnts",njnts);
       map_arm_joint_indices(js_msg.name);
    }
       for (int i=0;i<VECTOR_DIM;i++)
       {
        g_q_vec[i] = js_msg.position[g_arm_joint_indices[i]];
        }
        cout<<"CB: q_vec_right_arm: "<<g_q_vec.transpose()<<endl;       
    
}
Example #22
0
 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;
     }
     if (_mean_function.h_params_size() > 0) {
         Eigen::MatrixXd m_grad = Eigen::MatrixXd::Zero(_tr.rows() + 1, _mean_function.h_params_size());
         m_grad.block(0, 0, _tr.rows(), _mean_function.h_params_size()) = _mean_function.grad(x, gp);
         Eigen::MatrixXd gg = _tr * m_grad;
         grad.block(0, h_params_size() - _mean_function.h_params_size(), _tr.rows(), _mean_function.h_params_size()) = gg;
     }
     return grad;
 }
Eigen::MatrixXd get_symmetric_point(
    const Eigen::VectorXd& _normal,
    const Eigen::VectorXd& _center,
    const Eigen::MatrixXd& _points)
{
    // Assume that '_normal' is normalized.
    Eigen::MatrixXd plane_to_points = _normal * _normal.transpose() * (_points.colwise() - _center);
    Eigen::MatrixXd symmetric_point = _points - (plane_to_points * 2);

    // Debug.
    //Eigen::MatrixXd mid_points = 0.5 * (symmetric_point + _points);
    //Eigen::VectorXd distances = (mid_points.colwise() - _center).transpose() * _normal;
    //std::cout << distances.sum() / distances.rows() << std::endl;

    return symmetric_point;
}
Example #24
0
		Eigen::VectorXd LinearModel::predict(const vector<double> & substance, bool transform)
		{
			if (training_result_.rows() == 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 = v.transpose()*training_result_;
			//if (offsets_.getSize() == res.getSize()) res -= offsets_; 

			if (transform && y_transformations_.cols() != 0)
			{
				backTransformPrediction(res); 
			}
			
			return res;
		}
Example #25
0
void MyWindow::keyboard(unsigned char key, int x, int y) {
  static bool inverse = false;
	static const double dDOF = 0.1;
	switch(key) {
		case '-': {
			inverse = !inverse;
		}	break;
		case '1':
		case '2': 
		case '3': {
			size_t dofIdx = key - 49;
			Eigen::VectorXd pose = skel->get_q();
			pose(dofIdx) = pose(dofIdx) + (inverse ? -dDOF : dDOF);
			skel->setPose(pose);
			std::cout << "Updated pose DOF " << dofIdx << ": " << pose.transpose() << std::endl;
			glutPostRedisplay();
		}	break;
		default:
			Win3D::keyboard(key, x, y);
	}
	glutPostRedisplay();
}
Example #26
0
/* ********************************************************************************************* */
void moveFoot(const Eigen::VectorXd& dx, bool left, Vector6d& dq) {

	// Get the jacobian
	Eigen::MatrixXd J = hubo->getBodyNode(left ? "leftFoot" : "rightFoot")
		->getWorldJacobian().bottomRightCorner<6,6>();
	Eigen::MatrixXd temp = J.topRightCorner<3,6>();
	J.topRightCorner<3,6>() = J.bottomRightCorner<3,6>();
	J.bottomRightCorner<3,6>() = temp;
	for(size_t i = 0; i < 6; i++) J(i,i) += 0.005;
	if(dbg) std::cout << "J= [\n" << J << "];\n";

	// Compute the inverse
	Eigen::MatrixXd JInv;
	JInv = J;
	aa_la_inv(6, JInv.data());

	// Compute joint space velocity
	if(dbg) cout << "dxRightLeg: " << dx.transpose() << endl;
	dq = (JInv * dx);
	if(dq.norm() > max_step_size) dq = dq.normalized() * max_step_size;
	if(dbg) cout << "dqRightLeg: " << dq.transpose() << endl;
}
Example #27
0
      // see Rasmussen and Williams, 2006 (p. 114)
      Eigen::VectorXd log_likelihood_grad(const Eigen::VectorXd& h_params,
                                          bool update_kernel = true) {
        this->_kernel_function.set_h_params(h_params);
        if (update_kernel)
          this->_compute_kernel();
        size_t n = this->_observations.size();

        /// what we should write, but it is less numerically stable than using the Cholesky decomposition
        // Eigen::MatrixXd alpha = this->_inverted_kernel * this->_obs_mean;
        //  Eigen::MatrixXd w = alpha * alpha.transpose() - this->_inverted_kernel;

        // alpha = K^{-1} * this->_obs_mean;
        Eigen::VectorXd alpha = this->_llt.matrixL().solve(this->_obs_mean);
        this->_llt.matrixL().adjoint().solveInPlace(alpha);

        // K^{-1} using Cholesky decomposition
        Eigen::MatrixXd w = Eigen::MatrixXd::Identity(n, n);
        this->_llt.matrixL().solveInPlace(w);
        this->_llt.matrixL().transpose().solveInPlace(w);

        // alpha * alpha.transpose() - K^{-1}
        w = alpha * alpha.transpose() - w;

        // only compute half of the matrix (symmetrical matrix)
        Eigen::VectorXd grad =
          Eigen::VectorXd::Zero(this->_kernel_function.h_params_size());
        for (size_t i = 0; i < n; ++i) {
          for (size_t j = 0; j <= i; ++j) {
            Eigen::VectorXd g = this->_kernel_function.grad(this->_samples[i], this->_samples[j]);
            if (i == j)
              grad += w(i, j) * g * 0.5;
            else
              grad += w(i, j) * g;
          }
        }
        return grad;
      }
	void object::test<6>()
	{
		for (int i = 0;i<10;i++)
		{
			int dim = rand()%1000+2;
			int samplenum1 = rand()%1000+100;
			int samplenum2 = rand()%1000+100;

			Eigen::MatrixXd ha = Eigen::MatrixXd::Random(dim,samplenum1);
			Eigen::MatrixXd hb = Eigen::MatrixXd::Random(dim,samplenum2);



			Eigen::VectorXd haa = (ha.array()*ha.array()).colwise().sum();
			Eigen::VectorXd hbb = (hb.array()*hb.array()).colwise().sum();

			Eigen::MatrixXd hdist = -2*ha.transpose()*hb;

			hdist.colwise() += haa;

			hdist.rowwise() += hbb.transpose();

			
			Matrix<double> ga(ha),gb(hb);

			Matrix<double> gdist = -2*ga.transpose()*gb;

			Vector<double> gaa = (ga.array()*ga.array()).colwise().sum();
			Vector<double> gbb = (gb.array()*gb.array()).colwise().sum();

			gdist.colwise() += gaa;
			gdist.rowwise() += gbb;

			ensure(check_diff(hdist,gdist));
		}
	}
Example #29
0
//==============================================================================
std::string toString(const Eigen::VectorXd& _v)
{
  return boost::lexical_cast<std::string>(_v.transpose());
}
Example #30
0
Eigen::VectorXd RBM::operator()(const Eigen::VectorXd& x)
{
  v = x.transpose();
  sampleHgivenV();
  return ph.transpose();
}