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