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;
}
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;
}
/* ********************************************************************************************* */
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;
}
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;
}
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 ¶meter) {
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());
}
}
/** 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;
}
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;
}
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;
}
}
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]++;
}
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));
}
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;
}
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;
}
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;
}
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;
}
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;
}
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();
}
/* ********************************************************************************************* */
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;
}
// 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));
}
}
//==============================================================================
std::string toString(const Eigen::VectorXd& _v)
{
return boost::lexical_cast<std::string>(_v.transpose());
}
Eigen::VectorXd RBM::operator()(const Eigen::VectorXd& x)
{
v = x.transpose();
sampleHgivenV();
return ph.transpose();
}
