void Human::computeRGT()
{
//double maxRGT = 0;
for (int i = 0; i < waitingTimeSequence.size(); ++i) {
double time_ = waitingTimeSequence[i].first;
int visitIndex = waitingTimeSequence[i].second; //the number of visited locations
//apply the equation to calculate rg_t
Point r_cm(0, 0);
for (int j = 0; j < visitIndex; ++j) {
Point p = visitedLocations[j].position;
r_cm.x += p.x;
r_cm.y += p.y;
}
r_cm.x = r_cm.x/visitIndex;
r_cm.y = r_cm.y/visitIndex;
double dist = 0;
for (int j = 0; j < visitIndex; ++j) {
Point p = visitedLocations[j].position;
dist += p.distance2(r_cm); //distance square
}
dist = dist/visitIndex;
double rgt = sqrt(dist);
rg_t.push_back(make_pair(time_, rgt));
if (maxRGT < rgt) {
maxRGT = rgt;
}
}
//Normalize
//for (int i = 0; i < rg_t.size(); ++i) {
// rg_t[i].second = rg_t[i].second/maxRGT;
//}
}
/* Used in calculating system energy in the energy conservation tests */
Eigen::Vector3d
ckbot::chain::get_linear_velocity(int link_num)
{
Eigen::Vector3d vel(0,0,0);
Eigen::Vector3d vel_tip(0,0,0);
Eigen::Vector3d v_prev(0,0,0);
Eigen::Vector3d omega(0,0,0);
Eigen::Matrix3d R_prev = Eigen::Matrix3d::Identity();
Eigen::Vector3d r_jt_jt(0,0,0);
Eigen::Vector3d r_cm(0,0,0);
for (int i = 0; i <= link_num; i++)
{
R_prev = (ckbot::rotZ(links_[i].get_q())*links_[i].get_R_jts()).transpose();
r_jt_jt = -links_[i].get_r_im1() + links_[i].get_r_ip1();
r_cm = -links_[i].get_r_im1();
v_prev = R_prev*vel_tip;
omega = R_prev*(omega + Eigen::Vector3d(0,0,links_[i].get_qd())); //6DOF chainge
vel_tip = v_prev + omega.cross(r_jt_jt);
vel = v_prev + omega.cross(r_cm);
}
return vel;
}