void update() {
// Redraw screen now, please, and call again in 15ms.
glutPostRedisplay();
glutTimerFunc(15, updateI, 0);
// Input is win32 only
HWND window = GetActiveWindow();
if(window == GetForegroundWindow()) {
POINT p;
GetCursorPos(&p);
ScreenToClient(window, &p);
glutWarpPointer(WINDOW_WIDTH / 2, WINDOW_HEIGHT / 2);
float angleX = (p.x - (WINDOW_WIDTH / 2)) * CAM_ROTSPEED;
Quaternion rotX = RotationQuaternion(-angleX, MakeVector(0, 1, 0));
camera.front = TransformVector(RotationMatrixFromQuaternion(rotX), camera.front);
camera.elevation += (p.y - (WINDOW_HEIGHT / 2)) * CAM_ROTSPEED;
camera.elevation = max(-0.9f, min(camera.elevation, 0.9f));
if(GetAsyncKeyState('W') != 0) {
camera.pos = VectorAdd(camera.pos, VectorMul(camera.front, CAM_MOVESPEED));
}
if(GetAsyncKeyState('S') != 0) {
camera.pos = VectorAdd(camera.pos, VectorMul(camera.front, -CAM_MOVESPEED));
}
if(GetAsyncKeyState('E') != 0) {
camera.pos = VectorAdd(camera.pos, VectorMul(camera.up, CAM_MOVESPEED));
}
if(GetAsyncKeyState('Q') != 0) {
camera.pos = VectorAdd(camera.pos, VectorMul(camera.up, -CAM_MOVESPEED));
}
if(GetAsyncKeyState('D') != 0) {
camera.pos = VectorAdd(camera.pos, VectorMul(VectorCross(camera.front, camera.up), CAM_MOVESPEED));
}
if(GetAsyncKeyState('A') != 0) {
camera.pos = VectorAdd(camera.pos, VectorMul(VectorCross(camera.front, camera.up), -CAM_MOVESPEED));
}
}
}
bool TorsoControlStaticGait::advance(double dt) {
comControl_->advance(dt);
// Get measured orientation
const RotationQuaternion orientationWorldToControl = torso_->getMeasuredState().getOrientationWorldToControl(); // --> current heading orientation
/********************************************************************************************************
* Set desired base position in world frame
********************************************************************************************************/
// this is the position we need to compute:
Position positionControlToTargetBaseInControlFrame;
/* Compute the horizontal component of the desired position in world frame.
*
* evaluate desired CoM position in control frame
*/
Position positionWorldToDesiredHorizontalBaseInWorldFrame = comControl_->getPositionWorldToDesiredCoMInWorldFrame();
// this is the desired location of the base location relative to the origin of the control frame projected on the x-y plane of the world frame and expressed in the world frame
Position positionHorizontalControlToHorizontalBaseInWorldFrame = positionWorldToDesiredHorizontalBaseInWorldFrame
- torso_->getMeasuredState().getPositionWorldToControlInWorldFrame();
positionHorizontalControlToHorizontalBaseInWorldFrame.z() = 0.0;
/*************************************************************************
* Method I - NEW
*
* The desired base position has to lie on the line:
* W_r_C_B* = W_r_C_Bh* + lambda*W_e_z^W
* and on the plane parallel to the surface (with normal W_n) with
* the desired height above ground (h*):
* C_n * (C_r_C_B* - C_n * h*) = 0
*
* The intersection point is given by:
* C_n * ( C_n * h* - C_r_C_Bh*)
* C_r_C_B* = C_r_C_Bh* + ----------------------------- * C_e_z^W
* C_n * C_e_z^W
*
* We compute it in control frame instead of world frame.
*
*************************************************************************/
// // this is the surface normal
// loco::Vector surfaceNormalInWorldFrame;
// terrain_->getNormal(loco::Position::Zero(), surfaceNormalInWorldFrame);
// loco::Vector surfaceNormalInControlFrame = orientationWorldToControl.rotate(surfaceNormalInWorldFrame);
//
// const loco::Vector verticalAxisOfWorldFrameInWorldFrame = loco::Vector::UnitZ();
// const loco::Vector verticalAxisOfWorldFrameInControlFrame = orientationWorldToControl.rotate(verticalAxisOfWorldFrameInWorldFrame);
//
//
// const Position positionHorizontalControlToHorizontalBaseInControlFrame = orientationWorldToControl.rotate(positionHorizontalControlToHorizontalBaseInWorldFrame);
//
// Position temp2 = (desiredTorsoCoMHeightAboveGroundInControlFrameOffset_*Position(surfaceNormalInControlFrame))
// - positionHorizontalControlToHorizontalBaseInControlFrame;
// double scaleNumerator = Position(surfaceNormalInControlFrame).dot(temp2);
// double scaleDenominator = Position(surfaceNormalInControlFrame).dot(verticalAxisOfWorldFrameInControlFrame);
// double scale = scaleNumerator/scaleDenominator;
//
// positionControlToTargetBaseInControlFrame = orientationWorldToControl.rotate(positionHorizontalControlToHorizontalBaseInWorldFrame)
// + Position(verticalAxisOfWorldFrameInControlFrame*scale);
/*************************************************************************
* Method II - OLD
*
* The desired base position has to lie on the line:
* W_r_C_B* = W_r_C_Bh* + lambda*W_e_z^W
*
* and the vertical component in world frame has to be equal to the sum of
* the height of the terrain where this line goes through and the desired
* height of the torso above ground.
*
*************************************************************************/
RotationQuaternion orientationWorldToTerrain = getOrientationWorldToHeadingOnTerrainSurface(RotationQuaternion());
Position positionWorldToDesiredHeightAboveTerrainInTerrainFrame(0.0, 0.0, desiredTorsoCoMHeightAboveGroundInControlFrameOffset_);
Position positionWorldToDesiredHeightAboveTerrainInWorldFrame = orientationWorldToTerrain.inverseRotate(positionWorldToDesiredHeightAboveTerrainInTerrainFrame);
loco::Vector surfaceNormalInWorldFrame;
terrain_->getNormal(loco::Position::Zero(), surfaceNormalInWorldFrame);
double heightOverTerrain = positionWorldToDesiredHeightAboveTerrainInWorldFrame.dot(surfaceNormalInWorldFrame);
heightOverTerrain /= surfaceNormalInWorldFrame.z();
double heightOfTerrainInWorldFrame = 0.0;
terrain_->getHeight(positionWorldToDesiredHorizontalBaseInWorldFrame, heightOfTerrainInWorldFrame);
// Position positionWorldToHorizontalBaseInWorldFrame_temp = positionWorldToDesiredHorizontalBaseInWorldFrame + heightOfTerrainInWorldFrame*Position::UnitZ();
Position positionControlToTargetBaseInWorldFrame = positionHorizontalControlToHorizontalBaseInWorldFrame
+ (heightOfTerrainInWorldFrame + heightOverTerrain)*Position::UnitZ();
positionControlToTargetBaseInControlFrame = orientationWorldToControl.rotate(positionControlToTargetBaseInWorldFrame + desiredPositionOffsetInWorldFrame_);
/********************************************************************************************************
* End set desired CoM position in world frame *
********************************************************************************************************/
/********************************************************************************************************
* Set the desired orientation of the base frame with respect to the control frame
********************************************************************************************************/
// this is the orientation we need to compute
RotationQuaternion orientationControlToDesiredBase;
/*************************************************************************
* Method I - NEW
*
* Assumes that the control frame is aligned with the terrain surface.
* If the orientation of the body should be fully adapted to the terrain,
* we don't have to do anything.
*
* The desired orientation will be given by a composition of rotations:
* --> rotation from world to current heading (starting point for the composition).
* --> rotation from current heading to desired heading. This can be due to the body yaw induced by the foot holds.
* --> rotation from desired heading to desired base. This is an adaption to the terrain's estimated pitch and roll angles.
*
*************************************************************************/
RotationQuaternion orientationCurrentHeadingToDesiredHeading = getOrientationHeadingToDesiredHeadingBasedOnFeetLocations(positionWorldToDesiredHorizontalBaseInWorldFrame);
//--- Compose rotations
orientationControlToDesiredBase = desiredOrientationOffset_*orientationCurrentHeadingToDesiredHeading;
//---
/*************************************************************************
* Method II - OLD
*
*************************************************************************/
// RotationQuaternion orientationCurrentHeadingToDesiredHeading = getOrientationHeadingToDesiredHeadingBasedOnFeetLocations(positionWorldToDesiredHorizontalBaseInWorldFrame);
// RotationQuaternion orientationWorldToDesiredBase = getOrientationWorldToHeadingOnTerrainSurface(orientationCurrentHeadingToDesiredHeading*getOrientationWorldToHeadingBasedOnHipLocations());
// orientationControlToDesiredBase = orientationWorldToDesiredBase*orientationWorldToControl.inverted();
/*******************************
* End set desired orientation *
*******************************/
torso_->getDesiredState().setPositionControlToBaseInControlFrame(positionControlToTargetBaseInControlFrame);
torso_->getDesiredState().setOrientationControlToBase(orientationControlToDesiredBase);
// EulerAnglesZyx torsoAttitude = EulerAnglesZyx(orientationControlToDesiredBase).getUnique();
// std::cout << "*******" << std::endl;
// std::cout << "Desired torso position in control frame: " << std::endl << positionControlToTargetBaseInControlFrame << std::endl;
// std::cout << "Desired torso attitude in control frame: " << std::endl << torsoAttitude.roll() << " "
// << torsoAttitude.pitch() << " "
// << torsoAttitude.yaw() << std::endl;
// std::cout << "*******" << std::endl << std::endl;
/*************************************************************************
* Method II - OLD
* The input torso_->getDesiredState().setLinearVelocityBaseInControlFrame is actually not in control frame!!!!
* We need to correct this with a hack (fixme in future)
*
* velocities are given in frame HeadingOnTerrain
*
*************************************************************************/
// RotationQuaternion orientationWorldToHeadingOnTerrain = getOrientationWorldToHeadingOnTerrainSurface(getOrientationWorldToHeadingBasedOnHipLocations());
// RotationQuaternion orientationControlToHeadingOnTerrain = orientationWorldToControl*orientationWorldToHeadingOnTerrain.inverted();
// LinearVelocity linearVelocityBaseInControlFrame = orientationControlToHeadingOnTerrain.rotate(torso_->getDesiredState().getLinearVelocityBaseInControlFrame());
// torso_->getDesiredState().setLinearVelocityBaseInControlFrame(linearVelocityBaseInControlFrame);
LinearVelocity linearVelocityBaseInControlFrame;
LocalAngularVelocity angularVelocityBaseInControlFrame;
torso_->getDesiredState().setLinearVelocityBaseInControlFrame(linearVelocityBaseInControlFrame);
torso_->getDesiredState().setAngularVelocityBaseInControlFrame(angularVelocityBaseInControlFrame);
return true;
}
/*
* Peform a course registration using selected corresponding points by user
*/
void CourseRegistration(void)
{
register int i;
double Sxx, Sxy, Sxz,
Syx, Syy, Syz,
Szx, Szy, Szz;
double max_eval;
point_xyz *p, *q;
point_xyz mean_corres_p,
mean_corres_q;
int num_corres;
matrix *Q, *R;
vector *max_evec, *t;
// initialize values
Sxx = Sxy = Sxz = Syx = Syy = Syz = Szx = Szy = Szz = 0.0;
mean_corres_p.x = mean_corres_p.y = mean_corres_p.z = 0.0;
mean_corres_q.x = mean_corres_q.y = mean_corres_q.z = 0.0;
// take the smaller one
num_corres = (num_corres_anc<num_corres_mov)?num_corres_anc:num_corres_mov;
// allocate memory
Q = AllocateMatrix(4, 4);
R = AllocateMatrix(3, 3);
max_evec = AllocateVector(4);
t = AllocateVector(3);
p = (point_xyz *) malloc (num_corres * sizeof(point_xyz));
q = (point_xyz *) malloc (num_corres * sizeof(point_xyz));
// transform
for (i=0; i<num_corres; i++) {
p[i].x = (rd_mov[corres_rd_mov[i]].xyz[3*corres_pt_mov[i]] *
rd_mov[corres_rd_mov[i]].M[0]) +
(rd_mov[corres_rd_mov[i]].xyz[3*corres_pt_mov[i]+1] *
rd_mov[corres_rd_mov[i]].M[4]) +
(rd_mov[corres_rd_mov[i]].xyz[3*corres_pt_mov[i]+2] *
rd_mov[corres_rd_mov[i]].M[8]) +
rd_mov[corres_rd_mov[i]].M[12];
p[i].y = (rd_mov[corres_rd_mov[i]].xyz[3*corres_pt_mov[i]] *
rd_mov[corres_rd_mov[i]].M[1]) +
(rd_mov[corres_rd_mov[i]].xyz[3*corres_pt_mov[i]+1] *
rd_mov[corres_rd_mov[i]].M[5]) +
(rd_mov[corres_rd_mov[i]].xyz[3*corres_pt_mov[i]+2] *
rd_mov[corres_rd_mov[i]].M[9]) +
rd_mov[corres_rd_mov[i]].M[13];
p[i].z = (rd_mov[corres_rd_mov[i]].xyz[3*corres_pt_mov[i]] *
rd_mov[corres_rd_mov[i]].M[2]) +
(rd_mov[corres_rd_mov[i]].xyz[3*corres_pt_mov[i]+1] *
rd_mov[corres_rd_mov[i]].M[6]) +
(rd_mov[corres_rd_mov[i]].xyz[3*corres_pt_mov[i]+2] *
rd_mov[corres_rd_mov[i]].M[10]) +
rd_mov[corres_rd_mov[i]].M[14];
q[i].x = (rd_anc[corres_rd_anc[i]].xyz[3*corres_pt_anc[i]] *
rd_anc[corres_rd_anc[i]].M[0]) +
(rd_anc[corres_rd_anc[i]].xyz[3*corres_pt_anc[i]+1] *
rd_anc[corres_rd_anc[i]].M[4]) +
(rd_anc[corres_rd_anc[i]].xyz[3*corres_pt_anc[i]+2] *
rd_anc[corres_rd_anc[i]].M[8]) +
rd_anc[corres_rd_anc[i]].M[12];
q[i].y = (rd_anc[corres_rd_anc[i]].xyz[3*corres_pt_anc[i]] *
rd_anc[corres_rd_anc[i]].M[1]) +
(rd_anc[corres_rd_anc[i]].xyz[3*corres_pt_anc[i]+1] *
rd_anc[corres_rd_anc[i]].M[5]) +
(rd_anc[corres_rd_anc[i]].xyz[3*corres_pt_anc[i]+2] *
rd_anc[corres_rd_anc[i]].M[9]) +
rd_anc[corres_rd_anc[i]].M[13];
q[i].z = (rd_anc[corres_rd_anc[i]].xyz[3*corres_pt_anc[i]] *
rd_anc[corres_rd_anc[i]].M[2]) +
(rd_anc[corres_rd_anc[i]].xyz[3*corres_pt_anc[i]+1] *
rd_anc[corres_rd_anc[i]].M[6]) +
(rd_anc[corres_rd_anc[i]].xyz[3*corres_pt_anc[i]+2] *
rd_anc[corres_rd_anc[i]].M[10]) +
rd_anc[corres_rd_anc[i]].M[14];
//printf("%d: %f %f %f\n", i, q[i].x, q[i].y, q[i].z);
}
for (i=0; i<num_corres; i++) {
mean_corres_p.x += p[i].x;
mean_corres_p.y += p[i].y;
mean_corres_p.z += p[i].z;
mean_corres_q.x += q[i].x;
mean_corres_q.y += q[i].y;
mean_corres_q.z += q[i].z;
}
mean_corres_p.x /= (double)num_corres;
mean_corres_p.y /= (double)num_corres;
mean_corres_p.z /= (double)num_corres;
mean_corres_q.x /= (double)num_corres;
mean_corres_q.y /= (double)num_corres;
mean_corres_q.z /= (double)num_corres;
for (i=0; i<num_corres; i++) {
Sxx += p[i].x * q[i].x;
Sxy += p[i].x * q[i].y;
Sxz += p[i].x * q[i].z;
Syx += p[i].y * q[i].x;
Syy += p[i].y * q[i].y;
Syz += p[i].y * q[i].z;
Szx += p[i].z * q[i].x;
Szy += p[i].z * q[i].y;
Szz += p[i].z * q[i].z;
}
Sxx = Sxx / (double)num_corres - (mean_corres_p.x * mean_corres_q.x);
Sxy = Sxy / (double)num_corres - (mean_corres_p.x * mean_corres_q.y);
Sxz = Sxz / (double)num_corres - (mean_corres_p.x * mean_corres_q.z);
Syx = Syx / (double)num_corres - (mean_corres_p.y * mean_corres_q.x);
Syy = Syy / (double)num_corres - (mean_corres_p.y * mean_corres_q.y);
Syz = Syz / (double)num_corres - (mean_corres_p.y * mean_corres_q.z);
Szx = Szx / (double)num_corres - (mean_corres_p.z * mean_corres_q.x);
Szy = Szy / (double)num_corres - (mean_corres_p.z * mean_corres_q.y);
Szz = Szz / (double)num_corres - (mean_corres_p.z * mean_corres_q.z);
// construct N
Q->entry[0][0] = Sxx + Syy + Szz;
Q->entry[1][0] = Q->entry[0][1] = Syz - Szy;
Q->entry[2][0] = Q->entry[0][2] = Szx - Sxz;
Q->entry[3][0] = Q->entry[0][3] = Sxy - Syx;
Q->entry[1][1] = Sxx - Syy - Szz;
Q->entry[1][2] = Q->entry[2][1] = Sxy + Syx;
Q->entry[1][3] = Q->entry[3][1] = Szx + Sxz;
Q->entry[2][2] = -Sxx + Syy - Szz;
Q->entry[2][3] = Q->entry[3][2] = Syz + Szy;
Q->entry[3][3] = -Sxx - Syy + Szz;
// --- compute largest eigenvalues and eigenvectors of Q ---
SymmetricLargestEigens(Q, max_evec, &max_eval);
// make sure max_evec[0] > 0
if (max_evec->entry[0] < 0) {
for (i=0; i<4; i++) max_evec->entry[i] *= -1.0;
}
// --- compute rotation matrix ---
RotationQuaternion(max_evec, R);
// --- compute translation vector ---
t->entry[0] = mean_corres_q.x -
R->entry[0][0] * mean_corres_p.x -
R->entry[0][1] * mean_corres_p.y -
R->entry[0][2] * mean_corres_p.z;
t->entry[1] = mean_corres_q.y -
R->entry[1][0] * mean_corres_p.x -
R->entry[1][1] * mean_corres_p.y -
R->entry[1][2] * mean_corres_p.z;
t->entry[2] = mean_corres_q.z -
R->entry[2][0] * mean_corres_p.x -
R->entry[2][1] * mean_corres_p.y -
R->entry[2][2] * mean_corres_p.z;
//PrintMatrix(R);
//PrintVector(t);
new_M[0] = R->entry[0][0]; new_M[4] = R->entry[0][1]; new_M[8] = R->entry[0][2];
new_M[1] = R->entry[1][0]; new_M[5] = R->entry[1][1]; new_M[9] = R->entry[1][2];
new_M[2] = R->entry[2][0]; new_M[6] = R->entry[2][1]; new_M[10] = R->entry[2][2];
new_M[12] = t->entry[0]; new_M[13] = t->entry[1]; new_M[14] = t->entry[2];
new_M[3] = new_M[7] = new_M[11] = 0; new_M[15] = 1;
// free memory
FreeMatrix(Q); FreeMatrix(R);
FreeVector(max_evec); FreeVector(t);
free(p); free(q);
}
Matrix Matrix::RotationYawPitchRoll(float yaw, float pitch, float roll)
{
Quaternion quaternion = Quaternion::RotationYawPitchRoll(yaw, pitch, roll);
return RotationQuaternion(quaternion);
}
