Ejemplo n.º 1
0
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));
		}
	}
}
Ejemplo n.º 2
0
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;
}
Ejemplo n.º 3
0
/*
 * 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);
}
Ejemplo n.º 4
0
		Matrix Matrix::RotationYawPitchRoll(float yaw, float pitch, float roll)
		{
			Quaternion quaternion = Quaternion::RotationYawPitchRoll(yaw, pitch, roll);
			return RotationQuaternion(quaternion);
		}