コード例 #1
0
SideConfidenceProvider::BallModelSideConfidence SideConfidenceProvider::computeCurrentBallConfidence()
{
  // Some constant parameters
  const float distanceObserved = lastBallObservation.norm();
  const float angleObserved = lastBallObservation.angle();
  const float& camZ = theCameraMatrix.translation.z();
  const float distanceAsAngleObserved = (pi_2 - std::atan2(camZ,distanceObserved));

  // Weighting for original pose
  float originalWeighting = computeAngleWeighting(angleObserved, theLocalizationTeamBall.position, theRobotPose,
                                                  standardDeviationBallAngle);
  originalWeighting *= computeDistanceWeighting(distanceAsAngleObserved, theLocalizationTeamBall.position, theRobotPose,
                                                camZ, standardDeviationBallDistance);

  // Weighting for mirrored pose
  const Pose2f  mirroredPose = Pose2f(pi) + (Pose2f)(theRobotPose);
  float mirroredWeighting = computeAngleWeighting(angleObserved, theLocalizationTeamBall.position, mirroredPose,
                                                  standardDeviationBallAngle);
  mirroredWeighting *= computeDistanceWeighting(distanceAsAngleObserved, theLocalizationTeamBall.position, mirroredPose,
                                                camZ, standardDeviationBallDistance);

  PLOT("module:SideConfidenceProvider:originalWeighting", originalWeighting);
  PLOT("module:SideConfidenceProvider:mirroredWeighting", mirroredWeighting);

  // Decide state based on weights
  if((mirroredWeighting < minWeighting) && (originalWeighting < minWeighting))
    return UNKNOWN;
  if((mirroredWeighting > weightingFactor * originalWeighting) || (originalWeighting > weightingFactor * mirroredWeighting))
    return mirroredWeighting > originalWeighting ? MIRROR : OK;
  return UNKNOWN;
}
コード例 #2
0
void TorsoMatrixProvider::update(TorsoMatrix& torsoMatrix)
{
  const Vector3f axis(theInertialData.angle.x(), theInertialData.angle.y(), 0.0f);
  const RotationMatrix torsoRotation = Rotation::AngleAxis::unpack(axis);

  // calculate "center of hip" position from left foot
  Pose3f fromLeftFoot = Pose3f(torsoRotation) *= theRobotModel.soleLeft;
  fromLeftFoot.translation *= -1.;
  fromLeftFoot.rotation = torsoRotation;

  // calculate "center of hip" position from right foot
  Pose3f fromRightFoot = Pose3f(torsoRotation) *= theRobotModel.soleRight;
  fromRightFoot.translation *= -1.;
  fromRightFoot.rotation = torsoRotation;

  // get foot z-rotations
  const Pose3f& leftFootInverse(theRobotModel.limbs[Limbs::footLeft].inverse());
  const Pose3f& rightFootInverse(theRobotModel.limbs[Limbs::footRight].inverse());
  const float leftFootZRotation = leftFootInverse.rotation.getZAngle();
  const float rightFootZRotation = rightFootInverse.rotation.getZAngle();

  // determine used foot
  const bool useLeft = fromLeftFoot.translation.z() > fromRightFoot.translation.z();
  torsoMatrix.leftSupportFoot = useLeft;

  // calculate foot span
  const Vector3f newFootSpan(fromRightFoot.translation - fromLeftFoot.translation);

  // and construct the matrix
  Pose3f newTorsoMatrix;
  newTorsoMatrix.translate(newFootSpan.x() / (useLeft ? 2.f : -2.f), newFootSpan.y() / (useLeft ? 2.f : -2.f), 0);
  newTorsoMatrix.conc(useLeft ? fromLeftFoot : fromRightFoot);

  // calculate torso offset
  if(torsoMatrix.translation.z() != 0) // the last torso matrix should be valid
  {
    Pose3f& torsoOffset = torsoMatrix.offset;
    torsoOffset = torsoMatrix.inverse();
    torsoOffset.translate(lastFootSpan.x() / (useLeft ? 2.f : -2.f), lastFootSpan.y() / (useLeft ? 2.f : -2.f), 0);
    torsoOffset.rotateZ(useLeft ? float(leftFootZRotation - lastLeftFootZRotation) : float(rightFootZRotation - lastRightFootZRotation));
    torsoOffset.translate(newFootSpan.x() / (useLeft ? -2.f : 2.f), newFootSpan.y() / (useLeft ? -2.f : 2.f), 0);
    torsoOffset.conc(newTorsoMatrix);
  }

  // adopt new matrix and footSpan
  static_cast<Pose3f&>(torsoMatrix) = newTorsoMatrix;
  lastLeftFootZRotation = leftFootZRotation;
  lastRightFootZRotation = rightFootZRotation;
  lastFootSpan = newFootSpan;

  // valid?
  torsoMatrix.isValid = theGroundContactState.contact;

  //
  PLOT("module:TorsoMatrixProvider:footSpanX", newFootSpan.x());
  PLOT("module:TorsoMatrixProvider:footSpanY", newFootSpan.y());
  PLOT("module:TorsoMatrixProvider:footSpanZ", newFootSpan.z());
}
コード例 #3
0
void GroundTruthEvaluator::update(GroundTruthResult& groundTruthResult)
{
  DECLARE_PLOT("module:GroundTruthEvaluator:translationalError");
  DECLARE_PLOT("module:GroundTruthEvaluator:rotationalError");
  DECLARE_PLOT("module:GroundTruthEvaluator:ballPositionError");
  DECLARE_PLOT("module:GroundTruthEvaluator:evaluationDelay");

  robotPoses.add(RepresentationWithTimestamp<RobotPose>(theRobotPose,
                 theFrameInfo.time));
  if(groundTruthRobotPoses.getNumberOfEntries() == 0 ||
     groundTruthRobotPoses.top().timestamp != theGroundTruthRobotPose.timestamp)
  {
    groundTruthRobotPoses.add(RepresentationWithTimestamp<RobotPose>(
                                theGroundTruthRobotPose, theGroundTruthRobotPose.timestamp));
  }

  RobotPose groundTruthRobotPose = currentGroundTruthRobotPose();
  RobotPose estimatedRobotPose = averageEstimatedRobotPose();

  float translationalError = (groundTruthRobotPose.translation
                              - estimatedRobotPose.translation).abs();
  PLOT("module:GroundTruthEvaluator:translationalError", translationalError);
  float rotationalError = fabs(groundTruthRobotPose.rotation
                               - estimatedRobotPose.rotation);
  if(rotationalError > pi) // Occures around +/-pi.
    rotationalError = pi2 - rotationalError;
  PLOT("module:GroundTruthEvaluator:rotationalError", rotationalError);

  ballModels.add(RepresentationWithTimestamp<BallModel>(theBallModel,
                 theFrameInfo.time));
  if(groundTruthBallModels.getNumberOfEntries() == 0 ||
     groundTruthBallModels.top().timestamp != theGroundTruthBallModel.timeWhenLastSeen)
  {
    groundTruthBallModels.add(RepresentationWithTimestamp<BallModel>(
                                theGroundTruthBallModel, theGroundTruthBallModel.timeWhenLastSeen));
  }

  BallModel groundTruthBallModel = currentGroundTruthBallModel();
  BallModel estimatedBallModel = averageEstimatedBallModel();

  float ballPositionError = (groundTruthBallModel.estimate.position
                             - estimatedBallModel.estimate.position).abs();
  PLOT("module:GroundTruthEvaluator:ballPositionError", ballPositionError);
  // TODO ball model velocity

  // TODO RobotsModel

  int evaluationDelay = 0;
  if(groundTruthRobotPoses.getNumberOfEntries() >= 2)
  {
    evaluationDelay = (int) theFrameInfo.time - (int) groundTruthRobotPoses[1].timestamp;
  }
  PLOT("module:GroundTruthEvaluator:evaluationDelay", evaluationDelay);
}
コード例 #4
0
ファイル: TeamDataProvider.cpp プロジェクト: CheddarB/nbites
void TeamDataProvider::update(TeamMateDataBH& teamMateData)
{
  DECLARE_PLOT("module:TeamDataProvider:ntpOffset1"); // 1-5: players
  DECLARE_PLOT("module:TeamDataProvider:ntpOffset2");
  DECLARE_PLOT("module:TeamDataProvider:ntpOffset3");
  DECLARE_PLOT("module:TeamDataProvider:ntpOffset4");
  DECLARE_PLOT("module:TeamDataProvider:ntpOffset5");

  PLOT("module:TeamDataProvider:ntpOffset1", ntp.timeSyncBuffers[1].bestOffset);
  PLOT("module:TeamDataProvider:ntpOffset2", ntp.timeSyncBuffers[2].bestOffset);
  PLOT("module:TeamDataProvider:ntpOffset3", ntp.timeSyncBuffers[3].bestOffset);
  PLOT("module:TeamDataProvider:ntpOffset4", ntp.timeSyncBuffers[4].bestOffset);
  PLOT("module:TeamDataProvider:ntpOffset5", ntp.timeSyncBuffers[5].bestOffset);

  teamMateData = theTeamMateDataBH;
  teamMateData.currentTimestamp = theFrameInfoBH.time;
  teamMateData.numOfConnectedTeamMates = 0;
  teamMateData.firstTeamMate = TeamMateDataBH::numOfPlayers;
  for(int i = TeamMateDataBH::firstPlayer; i < TeamMateDataBH::numOfPlayers; ++i)
  {
    teamMateData.isActive[i] = false;
    teamMateData.isFullyActive[i] = false;
    // Check, if network connection is working (-> connected):
    if(teamMateData.timeStamps[i] && theFrameInfoBH.getTimeSince(teamMateData.timeStamps[i]) < static_cast<int>(teamMateData.networkTimeout))
    {
      teamMateData.numOfConnectedTeamMates++;
      // Check, if team mate is not penalized (-> active):
      if(!teamMateData.isPenalized[i] && theOwnTeamInfoBH.players[i - 1].penalty == PENALTY_NONE)
      {
        teamMateData.numOfActiveTeamMates++;
        teamMateData.isActive[i] = true;
        if(teamMateData.numOfActiveTeamMates == 1)
          teamMateData.firstTeamMate = i;
        // Check, if team mate has not been fallen down or lost ground contact (-> fully active):
        if(teamMateData.hasGroundContact[i] && teamMateData.isUpright[i])
        {
          teamMateData.numOfFullyActiveTeamMates++;
          teamMateData.isFullyActive[i] = true;
        }
      }
    }
  }
  if(teamMateData.numOfConnectedTeamMates)
    teamMateData.wasConnected = theTeamMateDataBH.wasConnected = true;
  teamMateData.sendThisFrame = (ntp.doSynchronization(theFrameInfoBH.time, Global::getTeamOut()) ||
                                theFrameInfoBH.getTimeSince(lastSentTimeStamp) >= 200) // TODO config file?
#ifdef TARGET_ROBOT
                               && !(theMotionRequestBH.motion == MotionRequestBH::specialAction && theMotionRequestBH.specialActionRequest.specialAction == SpecialActionRequest::playDead)
                               && !(theMotionInfoBH.motion == MotionRequestBH::specialAction && theMotionInfoBH.specialActionRequest.specialAction == SpecialActionRequest::playDead)
#endif
                               ;
  if(teamMateData.sendThisFrame)
    lastSentTimeStamp = theFrameInfoBH.time;
}
コード例 #5
0
void TeamDataProvider::update(TeamMateData& teamMateData)
{
  DECLARE_PLOT("module:TeamDataProvider:sslVisionOffset");
  DECLARE_PLOT("module:TeamDataProvider:ntpOffset1"); // 1-4: players
  DECLARE_PLOT("module:TeamDataProvider:ntpOffset2");
  DECLARE_PLOT("module:TeamDataProvider:ntpOffset3");
  DECLARE_PLOT("module:TeamDataProvider:ntpOffset4");
  DECLARE_PLOT("module:TeamDataProvider:ntpOffset12"); // SSL vision data

  PLOT("module:TeamDataProvider:ntpOffset1", ntp.timeSyncBuffers[1].bestOffset);
  PLOT("module:TeamDataProvider:ntpOffset2", ntp.timeSyncBuffers[2].bestOffset);
  PLOT("module:TeamDataProvider:ntpOffset3", ntp.timeSyncBuffers[3].bestOffset);
  PLOT("module:TeamDataProvider:ntpOffset4", ntp.timeSyncBuffers[4].bestOffset);
  PLOT("module:TeamDataProvider:ntpOffset12", ntp.timeSyncBuffers[12].bestOffset);

  teamMateData = theTeamMateData;
  teamMateData.numOfConnectedPlayers = 0;
  teamMateData.firstTeamMate = teamMateData.secondTeamMate = teamMateData.thirdTeamMate = TeamMateData::numOfPlayers;
  for(int i = TeamMateData::firstPlayer; i < TeamMateData::numOfPlayers; ++i)
    if(teamMateData.timeStamps[i] && theFrameInfo.getTimeSince(teamMateData.timeStamps[i]) < networkTimeout)
    {
      if(!teamMateData.isPenalized[i] && teamMateData.hasGroundContact[i])
      {
        ++teamMateData.numOfConnectedPlayers;
        if(teamMateData.numOfConnectedPlayers == 1) teamMateData.firstTeamMate = i;
        else if(teamMateData.numOfConnectedPlayers == 2) teamMateData.secondTeamMate = i;
        else if(teamMateData.numOfConnectedPlayers == 3) teamMateData.thirdTeamMate = i;
      }
      else
        teamMateData.timeStamps[i] = 0;
    }
  if(teamMateData.numOfConnectedPlayers > 0)
    teamMateData.wasConnected = theTeamMateData.wasConnected = true;
  teamMateData.sendThisFrame = (ntp.doSynchronization(theFrameInfo.time, Global::getTeamOut()) ||
                                theFrameInfo.getTimeSince(lastSentTimeStamp) >= 100) // TODO config file?
#ifdef TARGET_ROBOT
                               && !(theMotionRequest.motion == MotionRequest::specialAction && theMotionRequest.specialActionRequest.specialAction == SpecialActionRequest::playDead)
                               && !(theMotionInfo.motion == MotionRequest::specialAction && theMotionInfo.specialActionRequest.specialAction == SpecialActionRequest::playDead)
#endif
                               ;
  if(teamMateData.sendThisFrame)
  {
    lastSentTimeStamp = theFrameInfo.time;
    TEAM_OUTPUT(idTeamMateIsPenalized, bin, (theRobotInfo.penalty != PENALTY_NONE));
    TEAM_OUTPUT(idTeamMateHasGroundContact, bin, (theGroundContactState.contact || !theDamageConfiguration.useGroundContactDetection));
    TEAM_OUTPUT(idTeamMateIsUpright, bin, (theFallDownState.state == theFallDownState.upright));
    if(theGroundContactState.contact || !theDamageConfiguration.useGroundContactDetection)
      TEAM_OUTPUT(idTeamMateTimeSinceLastGroundContact, bin, lastSentTimeStamp);
  }
}
コード例 #6
0
void MotionCombinator::update(OdometryData& odometryData)
{
  if(!theRobotInfo.hasFeature(RobotInfo::zGyro) || (theFallDownState.state != FallDownState::upright && theLegMotionSelection.targetMotion != MotionRequest::getUp))
    this->odometryData.rotation += theFallDownState.odometryRotationOffset;
  this->odometryData.rotation.normalize();

  odometryData = this->odometryData;

  Pose2f odometryOffset = odometryData;
  odometryOffset -= lastOdometryData;
  PLOT("module:MotionCombinator:odometryOffsetX", odometryOffset.translation.x());
  PLOT("module:MotionCombinator:odometryOffsetY", odometryOffset.translation.y());
  PLOT("module:MotionCombinator:odometryOffsetRotation", odometryOffset.rotation.toDegrees());
  lastOdometryData = odometryData;
}
コード例 #7
0
ファイル: main.c プロジェクト: antoneri/5FY033
/*
 * The spectrum of a simple AM wave
 */
void am_wave(void) {
    plot_t p;

    p.outfile = "output3.png";
    p.title = "AM wave";
    p.xlabel = "t [s]";
    p.ylabel = "u(t) [m]";
    p.xmin = 0;
    p.xmax = 0;
    init_gnuplot(&p);

    gsl_complex_packed_array data = calloc(2*N, sizeof(double));

    BEGIN_SCATTER(p, "u(t)");

    double t = 0;

    for (int i = 0; i < 2*N; i += 2) {
        data[i] = u(t);
        PLOT(p, t, data[i]);
        t += dt;
    }

    END_PLOT(p);

    p.outfile = "output4.png";
    p.title = "FFT of AM wave";
    p.xlabel = "f [Hz]";
    p.ylabel = "S(f) = |U(f)|^2 [m^2]";
    p.xmin = 0;
    p.xmax = 40;
    init_gnuplot(&p);

    gsl_fft_complex_radix2_forward(data, 1, N);

    BEGIN_PLOT(p, "S(f)");

    double f = 0;

    for (int i = 0; i < N; i += 2) {
        PLOT(p, f, (data[i]*data[i] + data[i+1]*data[i+1])/(N*N));
        f += 1.0;
    }

    END_PLOT(p);

    free(data);
}
コード例 #8
0
void BH2011GoalSymbols::update()
{
  timeSinceOppGoalWasSeen = (float) frameInfo.getTimeSince(goalPercept.timeWhenOppGoalLastSeen);
  timeSinceOwnGoalWasSeen = (float) frameInfo.getTimeSince(goalPercept.timeWhenOwnGoalLastSeen);
  timeSinceAnyGoalWasSeen = timeSinceOppGoalWasSeen < timeSinceOwnGoalWasSeen ? timeSinceOppGoalWasSeen : timeSinceOwnGoalWasSeen;
  oppGoalWasSeen          = timeSinceOppGoalWasSeen < 500;
  goalWasSeen             = timeSinceAnyGoalWasSeen < 500;

  /* Calculate seen goals in ready state*/
  if(gameInfo.state == STATE_READY)
  {
    if(frameInfo.getTimeSince(goalPercept.timeWhenOppGoalLastSeen) == 0 ||
       frameInfo.getTimeSince(goalPercept.timeWhenOwnGoalLastSeen) == 0)
      goalsSeenInReadyState += 1.0;
  }
  else
  {
    goalsSeenInReadyState = 0;
  }

  /* Calculate seen goals since last pickup */
  if((groundContactState.noContactSafe && damageConfiguration.useGroundContactDetectionForSafeStates)
     && (fallDownState.state == FallDownState::upright || fallDownState.state == FallDownState::staggering))
  {
    goalsSeenSinceLastPickup = 0.0;
  }
  else
  {
    if(frameInfo.getTimeSince(goalPercept.timeWhenOppGoalLastSeen) == 0 ||
       frameInfo.getTimeSince(goalPercept.timeWhenOwnGoalLastSeen) == 0)
      goalsSeenSinceLastPickup += 1.0;
  }
  PLOT("Behavior:goalsSeenSinceLastPickup", goalsSeenSinceLastPickup);

  /* determine angles to sides of free part of opponent goal */
  Vector2<> centerOfOppGoalAbs = Vector2<>((float) fieldDimensions.xPosOpponentGoalpost, (float)(fieldDimensions.yPosLeftGoal + fieldDimensions.yPosRightGoal) / 2.0f);
  Vector2<> centerOfOppGoalRel = Geometry::fieldCoord2Relative(robotPose, centerOfOppGoalAbs);
  float     leftDistance       = abs(centerOfOppGoalAbs.y - freePartOfOpponentGoalModel.leftEnd.y);
  float     rightDistance      = abs(centerOfOppGoalAbs.y - freePartOfOpponentGoalModel.rightEnd.y);
  if(leftDistance < rightDistance)
  {
    innerSide = freePartOfOpponentGoalModel.leftEnd;
    outerSide = freePartOfOpponentGoalModel.rightEnd;
  }
  else
  {
    innerSide = freePartOfOpponentGoalModel.rightEnd;
    outerSide = freePartOfOpponentGoalModel.leftEnd;
  }

  /* draw angles to free part into worldstate, see draw()-function below */
  DECLARE_DEBUG_DRAWING("behavior:GoalSymbols:FreePartAnglesOnField", "drawingOnField");
  COMPLEX_DRAWING("behavior:GoalSymbols:FreePartAnglesOnField", drawingOnField());
}
コード例 #9
0
/*
void TorsoMatrixProvider::update(FilteredOdometryOffset& odometryOffset)
{
  Pose2DBH odometryOffset;

  if(lastTorsoMatrix.translation.z != 0.)
  {
    Pose3DBH odometryOffset3D(lastTorsoMatrix);
    odometryOffset3D.conc(theTorsoMatrixBH.offset);
    odometryOffset3D.conc(theTorsoMatrixBH.invert());

    odometryOffset.translation.x = odometryOffset3D.translation.x;
    odometryOffset.translation.y = odometryOffset3D.translation.y;
    odometryOffset.rotation = odometryOffset3D.rotation.getZAngle();
  }

  PLOT("module:TorsoMatrixProvider:odometryOffsetX", odometryOffset.translation.x);
  PLOT("module:TorsoMatrixProvider:odometryOffsetY", odometryOffset.translation.y);
  PLOT("module:TorsoMatrixProvider:odometryOffsetRotation", toDegrees(odometryOffset.rotation));

  (Pose3DBH&)lastTorsoMatrix = theTorsoMatrixBH;
}
*/
void TorsoMatrixProvider::update(OdometryDataBH& odometryData)
{
  Pose2DBH odometryOffset;

  if(lastTorsoMatrix.translation.z != 0.)
  {
    Pose3DBH odometryOffset3D(lastTorsoMatrix);
    odometryOffset3D.conc(theTorsoMatrixBH.offset);
    odometryOffset3D.conc(theTorsoMatrixBH.invert());

    odometryOffset.translation.x = odometryOffset3D.translation.x;
    odometryOffset.translation.y = odometryOffset3D.translation.y;
    odometryOffset.rotation = odometryOffset3D.rotation.getZAngle();
  }

  PLOT("module:TorsoMatrixProvider:odometryOffsetX", odometryOffset.translation.x);
  PLOT("module:TorsoMatrixProvider:odometryOffsetY", odometryOffset.translation.y);
  PLOT("module:TorsoMatrixProvider:odometryOffsetRotation", toDegrees(odometryOffset.rotation));

  odometryData += odometryOffset;

  (Pose3DBH&)lastTorsoMatrix = theTorsoMatrixBH;
}
コード例 #10
0
void TorsoMatrixProvider::update(OdometryData& odometryData)
{
  Pose2f odometryOffset;

  if(lastTorsoMatrix.translation.z() != 0.)
  {
    Pose3f odometryOffset3D(lastTorsoMatrix);
    odometryOffset3D.conc(theTorsoMatrix.offset);
    odometryOffset3D.conc(theTorsoMatrix.inverse());

    odometryOffset.translation.x() = odometryOffset3D.translation.x();
    odometryOffset.translation.y() = odometryOffset3D.translation.y();
    odometryOffset.rotation = odometryOffset3D.rotation.getZAngle();
  }

  PLOT("module:TorsoMatrixProvider:odometryOffsetX", odometryOffset.translation.x());
  PLOT("module:TorsoMatrixProvider:odometryOffsetY", odometryOffset.translation.y());
  PLOT("module:TorsoMatrixProvider:odometryOffsetRotation", odometryOffset.rotation.toDegrees());

  odometryData += odometryOffset;

  (Pose3f&)lastTorsoMatrix = theTorsoMatrix;
}
コード例 #11
0
Vector4f LIPStateEstimator::measure(SupportFoot supportFoot, const Vector2f& LIPOrigin) const
{
  const Pose3f& supportFootToTorso = supportFoot == SupportFoot::left ? theRobotModel.soleLeft : theRobotModel.soleRight;
  const Quaternionf& torsoToWorld = theInertialData.orientation2D;
  const Pose3f originToTorso = supportFootToTorso + Vector3f(LIPOrigin.x(), LIPOrigin.y(), 0.f);
  const Vector3f comInOrigin = originToTorso.inverse() * theRobotModel.centerOfMass;
  const Vector3f com = (torsoToWorld * originToTorso.rotation) * comInOrigin;

  PLOT("module:ZmpWalkingEngine:LIPStateEstimator:Estimate:measuredComHeight", com.z());

  const Vector2f accInWorld = (torsoToWorld * theInertialData.acc * 1000.f).head<2>();
  const Vector2f zmp = com.head<2>().array() - (accInWorld.array() / LIP3D(LIPHeights).getK().square());

  return (Vector4f() << com.head<2>(), zmp).finished();
}
コード例 #12
0
struct GroundContactDetector::ContactState GroundContactDetector::checkLoad()
{
  ContactState state;

  float loadSum = theSensorData.currents[JointData::LHipPitch];
  loadSum += theSensorData.currents[JointData::LKneePitch];
  loadSum += theSensorData.currents[JointData::LAnklePitch];
  loadSum += theSensorData.currents[JointData::RHipPitch];
  loadSum += theSensorData.currents[JointData::RKneePitch];
  loadSum += theSensorData.currents[JointData::RAnklePitch];
  loadSum += theSensorData.currents[JointData::LHipRoll];
  loadSum += theSensorData.currents[JointData::LHipYawPitch];
  loadSum += theSensorData.currents[JointData::LAnkleRoll];
  loadSum += theSensorData.currents[JointData::RHipRoll];
  loadSum += theSensorData.currents[JointData::RHipYawPitch];
  loadSum += theSensorData.currents[JointData::RAnkleRoll];
  PLOT("module:GroundContactDetector:contactLoadSum", loadSum);

  state.contact = loadSum > p.loadThreshold;
  //max angleY: pi_4 (45°)
  state.confidence = 1 - min(abs(theSensorData.data[SensorData::angleY]), pi_4) / pi_4;
  return state;
}
コード例 #13
0
void InertialDataFilter::readingUpdate(const Vector3f& reading)
{
  generateSigmaPoints();

  for(int i = 0; i < 5; ++i)
    readingModel(sigmaPoints[i], sigmaReadings[i]);

  meanOfSigmaReadings();

  PLOT("module:InertialDataFilter:expectedAccX", readingMean.x());
  PLOT("module:InertialDataFilter:accX", reading.x());
  PLOT("module:InertialDataFilter:expectedAccY", readingMean.y());
  PLOT("module:InertialDataFilter:accY", reading.y());
  PLOT("module:InertialDataFilter:expectedAccZ", readingMean.z());
  PLOT("module:InertialDataFilter:accZ", reading.z());

  const Matrix3x2f readingsSigmaPointsCov = covOfSigmaReadingsAndSigmaPoints();
  const Matrix3f readingsCov = covOfSigmaReadings();

  const Matrix3f sensorCov = accNoise.array().square().matrix().asDiagonal();
  const Matrix2x3f kalmanGain = readingsSigmaPointsCov.transpose() * (readingsCov + sensorCov).inverse();
  mean += kalmanGain * (reading - readingMean);
  cov -= kalmanGain * readingsSigmaPointsCov;
}
コード例 #14
0
void ExpGroundContactDetector::update(GroundContactState& groundContactState)
{
  MODIFY("module:ExpGroundContactDetector:parameters", p);

  DECLARE_PLOT("module:ExpGroundContactDetector:angleNoiseX");
  DECLARE_PLOT("module:ExpGroundContactDetector:angleNoiseY");
  DECLARE_PLOT("module:ExpGroundContactDetector:accNoiseX");
  DECLARE_PLOT("module:ExpGroundContactDetector:accNoiseY");
  DECLARE_PLOT("module:ExpGroundContactDetector:accNoiseZ");
  DECLARE_PLOT("module:ExpGroundContactDetector:gyroNoiseX");
  DECLARE_PLOT("module:ExpGroundContactDetector:gyroNoiseY");

  MODIFY("module:ExpGroundContactDetector:contact", contact);
  bool ignoreSensors = (theMotionInfo.motion != MotionRequest::walk && theMotionInfo.motion != MotionRequest::stand) ||
                       (theMotionRequest.motion != MotionRequest::walk && theMotionRequest.motion != MotionRequest::stand) ||
                       (theMotionInfo.motion == MotionRequest::walk && theMotionInfo.walkRequest.kickType != WalkRequest::none);
  if(!ignoreSensors)
  {
    if(contact)
    {
      if(theInertiaSensorData.acc.z != InertiaSensorData::off)
        calibratedAccZValues.add(theInertiaSensorData.acc.z);

      Vector3<> angleDiff = ((const RotationMatrix&)(theTorsoMatrix.rotation * expectedRotationInv)).getAngleAxis();
      angleNoises.add(Vector2<>(sqr(angleDiff.x), sqr(angleDiff.y)));
      Vector2<> angleNoise = angleNoises.getAverage();
      PLOT("module:ExpGroundContactDetector:angleNoiseX", angleNoise.x);
      PLOT("module:ExpGroundContactDetector:angleNoiseY", angleNoise.y);

      if(!useAngle && angleNoises.isFull() && angleNoise.x < p.contactAngleActivationNoise && angleNoise.y < p.contactAngleActivationNoise)
        useAngle = true;

      if((useAngle && (angleNoise.x > p.contactMaxAngleNoise || angleNoise.y > p.contactMaxAngleNoise)) ||
        (calibratedAccZValues.isFull() && calibratedAccZValues.getAverage() > p.contactMaxAccZ))
      {
        /*
        if((useAngle && (angleNoise.x > p.contactMaxAngleNoise || angleNoise.y > p.contactMaxAngleNoise)))
          OUTPUT_ERROR("lost ground contact via angle");
        if((calibratedAccZValues.isFull() && calibratedAccZValues.getAverage() > p.contactMaxAccZ))
          OUTPUT_ERROR("lost ground contact via acc");
        */

        contact = false;
        accNoises.clear();
        gyroNoises.clear();
        accValues.clear();
        gyroValues.clear();
        angleNoises.clear();
#ifndef TARGET_SIM
        if(contactStartTime != 0)
          SoundPlayer::play("high.wav");
#endif
      }
    }
    else
    {
      Vector3<> accAverage = accValues.getAverage();
      Vector2<> gyroAverage = gyroValues.getAverage();
      if(theInspectedInertiaSensorData.acc.x != InertiaSensorData::off)
      {
        accValues.add(theInspectedInertiaSensorData.acc);
        gyroValues.add(theInspectedInertiaSensorData.gyro);
        if(accValues.isFull())
        {
          accNoises.add(Vector3<>(sqr(theInspectedInertiaSensorData.acc.x - accAverage.x), sqr(theInspectedInertiaSensorData.acc.y - accAverage.y), sqr(theInspectedInertiaSensorData.acc.z - accAverage.z)));
          gyroNoises.add(Vector2<>(sqr(theInspectedInertiaSensorData.gyro.x - gyroAverage.x), sqr(theInspectedInertiaSensorData.gyro.y - gyroAverage.y)));
        }
      }
      Vector3<> accNoise = accNoises.getAverage();
      Vector2<> gyroNoise = gyroNoises.getAverage();
      PLOT("module:ExpGroundContactDetector:accNoiseX", accNoise.x);
      PLOT("module:ExpGroundContactDetector:accNoiseY", accNoise.y);
      PLOT("module:ExpGroundContactDetector:accNoiseZ", accNoise.z);
      PLOT("module:ExpGroundContactDetector:gyroNoiseX", gyroNoise.x);
      PLOT("module:ExpGroundContactDetector:gyroNoiseY", gyroNoise.y);

      if(accNoises.isFull() &&
        accAverage.z < -5.f && std::abs(accAverage.x) < 5.f && std::abs(accAverage.y) < 5.f &&
        accNoise.x < p.noContactMinAccXNoise && accNoise.y < p.noContactMinAccYNoise && accNoise.z < p.noContactMinAccZNoise &&
        gyroNoise.x < p.noContactMinGyroNoise && gyroNoise.y < p.noContactMinGyroNoise)
      {
        contact = true;
        useAngle = false;
        contactStartTime = theFrameInfo.time;
        angleNoises.clear();
        calibratedAccZValues.clear();
      }
    }
  }

  groundContactState.contact = contact;
  
  expectedRotationInv = theRobotModel.limbs[MassCalibration::footLeft].translation.z > theRobotModel.limbs[MassCalibration::footRight].translation.z ? theRobotModel.limbs[MassCalibration::footLeft].rotation : theRobotModel.limbs[MassCalibration::footRight].rotation;
}
コード例 #15
0
void JointDataPrediction::draw() const
{
  DECLARE_PLOT("representation:JointDataPrediction:position:lHipYawPitch");
  DECLARE_PLOT("representation:JointDataPrediction:position:lHipRoll");
  DECLARE_PLOT("representation:JointDataPrediction:position:lHipPitch");
  DECLARE_PLOT("representation:JointDataPrediction:position:lKneePitch");
  DECLARE_PLOT("representation:JointDataPrediction:position:lAnklePitch");
  DECLARE_PLOT("representation:JointDataPrediction:position:lAnkleRoll");
  DECLARE_PLOT("representation:JointDataPrediction:position:rHipYawPitch");
  DECLARE_PLOT("representation:JointDataPrediction:position:rHipRoll");
  DECLARE_PLOT("representation:JointDataPrediction:position:rHipPitch");
  DECLARE_PLOT("representation:JointDataPrediction:position:rKneePitch");
  DECLARE_PLOT("representation:JointDataPrediction:position:rAnklePitch");
  DECLARE_PLOT("representation:JointDataPrediction:position:rAnkleRoll");

  DECLARE_PLOT("representation:JointDataPrediction:velocity:lHipYawPitch");
  DECLARE_PLOT("representation:JointDataPrediction:velocity:lHipRoll");
  DECLARE_PLOT("representation:JointDataPrediction:velocity:lHipPitch");
  DECLARE_PLOT("representation:JointDataPrediction:velocity:lKneePitch");
  DECLARE_PLOT("representation:JointDataPrediction:velocity:lAnklePitch");
  DECLARE_PLOT("representation:JointDataPrediction:velocity:lAnkleRoll");
  DECLARE_PLOT("representation:JointDataPrediction:velocity:rHipYawPitch");
  DECLARE_PLOT("representation:JointDataPrediction:velocity:rHipRoll");
  DECLARE_PLOT("representation:JointDataPrediction:velocity:rHipPitch");
  DECLARE_PLOT("representation:JointDataPrediction:velocity:rKneePitch");
  DECLARE_PLOT("representation:JointDataPrediction:velocity:rAnklePitch");
  DECLARE_PLOT("representation:JointDataPrediction:velocity:rAnkleRoll");

  DECLARE_PLOT("representation:JointDataPrediction:acceleration:lHipYawPitch");
  DECLARE_PLOT("representation:JointDataPrediction:acceleration:lHipRoll");
  DECLARE_PLOT("representation:JointDataPrediction:acceleration:lHipPitch");
  DECLARE_PLOT("representation:JointDataPrediction:acceleration:lKneePitch");
  DECLARE_PLOT("representation:JointDataPrediction:acceleration:lAnklePitch");
  DECLARE_PLOT("representation:JointDataPrediction:acceleration:lAnkleRoll");
  DECLARE_PLOT("representation:JointDataPrediction:acceleration:rHipYawPitch");
  DECLARE_PLOT("representation:JointDataPrediction:acceleration:rHipRoll");
  DECLARE_PLOT("representation:JointDataPrediction:acceleration:rHipPitch");
  DECLARE_PLOT("representation:JointDataPrediction:acceleration:rKneePitch");
  DECLARE_PLOT("representation:JointDataPrediction:acceleration:rAnklePitch");
  DECLARE_PLOT("representation:JointDataPrediction:acceleration:rAnkleRoll");

  PLOT("representation:JointDataPrediction:velocity:lHipYawPitch",
          velocities[Joints::lHipYawPitch]);
  PLOT("representation:JointDataPrediction:velocity:lHipRoll",
          velocities[Joints::lHipRoll]);
  PLOT("representation:JointDataPrediction:velocity:lHipPitch",
          velocities[Joints::lHipPitch]);
  PLOT("representation:JointDataPrediction:velocity:lKneePitch",
          velocities[Joints::lKneePitch]);
  PLOT("representation:JointDataPrediction:velocity:lAnklePitch",
          velocities[Joints::lAnklePitch]);
  PLOT("representation:JointDataPrediction:velocity:lAnkleRoll",
          velocities[Joints::lAnkleRoll]);
  PLOT("representation:JointDataPrediction:velocity:rHipYawPitch",
          velocities[Joints::rHipYawPitch]);
  PLOT("representation:JointDataPrediction:velocity:rHipRoll",
          velocities[Joints::rHipRoll]);
  PLOT("representation:JointDataPrediction:velocity:rHipPitch",
          velocities[Joints::rHipPitch]);
  PLOT("representation:JointDataPrediction:velocity:rKneePitch",
          velocities[Joints::rKneePitch]);
  PLOT("representation:JointDataPrediction:velocity:rAnklePitch",
          velocities[Joints::rAnklePitch]);
  PLOT("representation:JointDataPrediction:velocity:rAnkleRoll",
          velocities[Joints::rAnkleRoll]);

  PLOT("representation:JointDataPrediction:position:lHipYawPitch",
          angles[Joints::lHipYawPitch]);
  PLOT("representation:JointDataPrediction:position:lHipRoll",
          angles[Joints::lHipRoll]);
  PLOT("representation:JointDataPrediction:position:lHipPitch",
          angles[Joints::lHipPitch]);
  PLOT("representation:JointDataPrediction:position:lKneePitch",
          angles[Joints::lKneePitch]);
  PLOT("representation:JointDataPrediction:position:lAnklePitch",
          angles[Joints::lAnklePitch]);
  PLOT("representation:JointDataPrediction:position:lAnkleRoll",
          angles[Joints::lAnkleRoll]);
  PLOT("representation:JointDataPrediction:position:rHipYawPitch",
          angles[Joints::rHipYawPitch]);
  PLOT("representation:JointDataPrediction:position:rHipRoll",
          angles[Joints::rHipRoll]);
  PLOT("representation:JointDataPrediction:position:rHipPitch",
          angles[Joints::rHipPitch]);
  PLOT("representation:JointDataPrediction:position:rKneePitch",
          angles[Joints::rKneePitch]);
  PLOT("representation:JointDataPrediction:position:rAnklePitch",
          angles[Joints::rAnklePitch]);
  PLOT("representation:JointDataPrediction:position:rAnkleRoll",
          angles[Joints::rAnkleRoll]);

  PLOT("representation:JointDataPrediction:acceleration:lHipYawPitch",
          accelerations[Joints::lHipYawPitch]);
  PLOT("representation:JointDataPrediction:acceleration:lHipRoll",
          accelerations[Joints::lHipRoll]);
  PLOT("representation:JointDataPrediction:acceleration:lHipPitch",
          accelerations[Joints::lHipPitch]);
  PLOT("representation:JointDataPrediction:acceleration:lKneePitch",
          accelerations[Joints::lKneePitch]);
  PLOT("representation:JointDataPrediction:acceleration:lAnklePitch",
          accelerations[Joints::lAnklePitch]);
  PLOT("representation:JointDataPrediction:acceleration:lAnkleRoll",
          accelerations[Joints::lAnkleRoll]);
  PLOT("representation:JointDataPrediction:acceleration:rHipYawPitch",
          accelerations[Joints::rHipYawPitch]);
  PLOT("representation:JointDataPrediction:acceleration:rHipRoll",
          accelerations[Joints::rHipRoll]);
  PLOT("representation:JointDataPrediction:acceleration:rHipPitch",
          accelerations[Joints::rHipPitch]);
  PLOT("representation:JointDataPrediction:acceleration:rKneePitch",
          accelerations[Joints::rKneePitch]);
  PLOT("representation:JointDataPrediction:acceleration:rAnklePitch",
          accelerations[Joints::rAnklePitch]);
  PLOT("representation:JointDataPrediction:acceleration:rAnkleRoll",
          accelerations[Joints::rAnkleRoll]);

  DECLARE_DEBUG_DRAWING3D("representation:JointDataPrediction:com", "robot");
  CROSS3D("representation:JointDataPrediction:com", com.x(), com.y(), com.z(),
          50, 50, ColorRGBA::red);
  LINE3D("representation:JointDataPrediction:com", com.x(), com.y(), com.z(),
         com.x() + comAcceleration.x(), com.y() + comAcceleration.y(),
         com.z() + comAcceleration.z(), 10, ColorRGBA::red);

  DECLARE_PLOT("representation:JointDataPrediction:comAccX");
  DECLARE_PLOT("representation:JointDataPrediction:comAccY");
  DECLARE_PLOT("representation:JointDataPrediction:comAccZ");
  DECLARE_PLOT("representation:JointDataPrediction:comX");
  DECLARE_PLOT("representation:JointDataPrediction:comY");
  DECLARE_PLOT("representation:JointDataPrediction:comAtan2");
  DECLARE_PLOT("representation:JointDataPrediction:comAccAtan2");
  PLOT("representation:JointDataPrediction:comAccX", comAcceleration.x());
  PLOT("representation:JointDataPrediction:comAccY", comAcceleration.y());
  PLOT("representation:JointDataPrediction:comAccZ", comAcceleration.z());
  PLOT("representation:JointDataPrediction:comAccAtan2", atan2(comAcceleration.y(), comAcceleration.x()));
  PLOT("representation:JointDataPrediction:comX", com.x());
  PLOT("representation:JointDataPrediction:comY", com.y());
  PLOT("representation:JointDataPrediction:comAtan2", atan2(com.y(), com.x()));

  DECLARE_PLOT("representation:JointDataPrediction:angleSum");
  float sum = 0.0f;
  for(int i = 0; i < Joints::numOfJoints; ++i)
  {
    sum += angles[i];
  }
  PLOT("representation:JointDataPrediction:angleSum", sum);

  DECLARE_PLOT("representation:JointDataPrediction:squareVelSum");
  float velSum = 0.0f;
  for(int i = 0; i < Joints::numOfJoints; ++i)
  {
    velSum += velocities[i] * velocities[i];
  }
  PLOT("representation:JointDataPrediction:squareVelSum", velSum);
}
コード例 #16
0
void FootBumperStateProvider::update(FootBumperState& footBumperState)
{
  // Check, if any bumper is pressed
  const bool leftFootLeft = !theDamageConfigurationBody.sides[Legs::left].footBumperDefect && checkContact(KeyStates::leftFootLeft, leftFootLeftDuration);
  const bool leftFootRight = !theDamageConfigurationBody.sides[Legs::left].footBumperDefect && checkContact(KeyStates::leftFootRight, leftFootRightDuration);
  const bool rightFootLeft = !theDamageConfigurationBody.sides[Legs::right].footBumperDefect &&  checkContact(KeyStates::rightFootLeft, rightFootLeftDuration);
  const bool rightFootRight = !theDamageConfigurationBody.sides[Legs::right].footBumperDefect && checkContact(KeyStates::rightFootRight, rightFootRightDuration);
  const bool contactLeftFoot = leftFootLeft || leftFootRight;
  const bool contactRightFoot = rightFootLeft || rightFootRight;
  // Update statistics
  if(contactLeftFoot)
  {
    contactBufferLeft.push_front(1);
    contactDurationLeft++;
  }
  else
  {
    contactBufferLeft.push_front(0);
    contactDurationLeft = 0;
  }
  if(contactRightFoot)
  {
    contactBufferRight.push_front(1);
    contactDurationRight++;
  }
  else
  {
    contactBufferRight.push_front(0);
    contactDurationRight = 0;
  }

  // Generate model
  if((theMotionInfo.motion == MotionInfo::stand || theMotionInfo.motion == MotionInfo::walk) &&
     (theGameInfo.state == STATE_READY || theGameInfo.state == STATE_SET || theGameInfo.state == STATE_PLAYING) && //The bumper is used for configuration in initial
     (theFallDownState.state == FallDownState::upright))
  {
    if(contactBufferLeft.sum() > contactThreshold)
    {
      footBumperState.status[Legs::left].contact = true;
      footBumperState.status[Legs::left].contactDuration = contactDurationLeft;
      if(contactLeftFoot)
        footBumperState.status[Legs::left].lastContact = theFrameInfo.time;
    }
    else
    {
      footBumperState.status[Legs::left].contact = false;
      footBumperState.status[Legs::left].contactDuration = 0;
    }
    if(contactBufferRight.sum() > contactThreshold)
    {
      footBumperState.status[Legs::right].contact = true;
      footBumperState.status[Legs::right].contactDuration = contactDurationRight;
      if(contactRightFoot)
        footBumperState.status[Legs::right].lastContact = theFrameInfo.time;
    }
    else
    {
      footBumperState.status[Legs::right].contact = false;
      footBumperState.status[Legs::right].contactDuration = 0;
    }
  }
  else
  {
    footBumperState.status[Legs::left].contact = false;
    footBumperState.status[Legs::right].contact = false;
    footBumperState.status[Legs::left].contactDuration = 0;
    footBumperState.status[Legs::right].contactDuration = 0;
  }

  // Debugging stuff:

  if(debug && theFrameInfo.getTimeSince(lastSoundTime) > (int)soundDelay && (footBumperState.status[Legs::left].contact || footBumperState.status[Legs::right].contact))
  {
    lastSoundTime = theFrameInfo.time;
    SystemCall::playSound("jump.wav");
  }

  PLOT("module:FootBumperStateProvider:sumLeft", contactBufferLeft.sum());
  PLOT("module:FootBumperStateProvider:durationLeft", contactDurationLeft);
  PLOT("module:FootBumperStateProvider:sumRight", contactBufferRight.sum());
  PLOT("module:FootBumperStateProvider:durationRight", contactDurationRight);
  PLOT("module:FootBumperStateProvider:contactLeft", footBumperState.status[Legs::left].contact ? 10 : 0);
  PLOT("module:FootBumperStateProvider:contactRight", footBumperState.status[Legs::right].contact ? 10 : 0);
  PLOT("module:FootBumperStateProvider:leftFootLeft", leftFootLeft ? 10 : 0);
  PLOT("module:FootBumperStateProvider:leftFootRight", leftFootRight ? 10 : 0);
  PLOT("module:FootBumperStateProvider:rightFootLeft", rightFootLeft ? 10 : 0);
  PLOT("module:FootBumperStateProvider:rightFootRight", rightFootRight ? 10 : 0);
}
コード例 #17
0
void GroundContactDetector::update(GroundContactState& groundContactState)
{
  DECLARE_PLOT("module:GroundContactDetector:angleNoiseX");
  DECLARE_PLOT("module:GroundContactDetector:angleNoiseY");
  DECLARE_PLOT("module:GroundContactDetector:accNoiseX");
  DECLARE_PLOT("module:GroundContactDetector:accNoiseY");
  DECLARE_PLOT("module:GroundContactDetector:accNoiseZ");
  DECLARE_PLOT("module:GroundContactDetector:gyroNoiseX");
  DECLARE_PLOT("module:GroundContactDetector:gyroNoiseY");

  MODIFY("module:GroundContactDetector:contact", contact);

  if(theMotionInfo.motion == MotionRequest::getUp) //hack to avoid long pause after get up 
  {
    contact = true;
    useAngle = false;
    groundContactState.contact = contact;
    contactStartTime = theFrameInfo.time;
  }

  bool ignoreSensors = (theMotionInfo.motion != MotionRequest::walk && theMotionInfo.motion != MotionRequest::stand &&
                        theMotionInfo.motion != MotionRequest::specialAction && theMotionInfo.motion != MotionRequest::getUp) ||
                       (theMotionRequest.motion != MotionRequest::walk && theMotionRequest.motion != MotionRequest::stand &&
                        theMotionRequest.motion != MotionRequest::specialAction && theMotionRequest.motion != MotionRequest::getUp) ||
                       (theMotionInfo.motion == MotionRequest::walk && theMotionInfo.walkRequest.kickType != WalkRequest::none) ||
                       (theMotionRequest.motion == MotionRequest::walk && theMotionRequest.walkRequest.kickType != WalkRequest::none);
  if(!ignoreSensors)
  {
    if(contact)
    {
      calibratedAccZValues.add(theSensorData.data[SensorData::accZ]);

      Vector3<> angleDiff = ((const RotationMatrix&)(theTorsoMatrix.rotation * expectedRotationInv)).getAngleAxis();
      angleNoises.add(Vector2<>(sqr(angleDiff.x), sqr(angleDiff.y)));
      Vector2<> angleNoise = angleNoises.getAverage();
      PLOT("module:GroundContactDetector:angleNoiseX", angleNoise.x);
      PLOT("module:GroundContactDetector:angleNoiseY", angleNoise.y);

      if(!useAngle && angleNoises.isFull() && angleNoise.x < contactAngleActivationNoise && angleNoise.y < contactAngleActivationNoise)
        useAngle = true;

      if((useAngle && (angleNoise.x > contactMaxAngleNoise || angleNoise.y > contactMaxAngleNoise)) ||
         (calibratedAccZValues.isFull() && calibratedAccZValues.getAverage() > contactMaxAccZ))
      {
        /*
        if((useAngle && (angleNoise.x > p.contactMaxAngleNoise || angleNoise.y > p.contactMaxAngleNoise)))
          OUTPUT_ERROR("lost ground contact via angle");
        if((calibratedAccZValues.isFull() && calibratedAccZValues.getAverage() > p.contactMaxAccZ))
          OUTPUT_ERROR("lost ground contact via acc");
        */

        contact = false;
        accNoises.clear();
        gyroNoises.clear();
        accValues.clear();
        gyroValues.clear();
        angleNoises.clear();
        if(SystemCall::getMode() == SystemCall::physicalRobot && contactStartTime != 0)
          SystemCall::playSound("high.wav");
      }
    }
    else
    {
      const Vector3<> accAverage = accValues.getAverage();
      const Vector2<> gyroAverage = gyroValues.getAverage();
      const Vector2<> gyro = Vector2<>(theSensorData.data[SensorData::gyroX], theSensorData.data[SensorData::gyroY]);
      const Vector3<> acc = Vector3<>(theSensorData.data[SensorData::accX], theSensorData.data[SensorData::accY], theSensorData.data[SensorData::accZ]);
      accValues.add(acc);
      gyroValues.add(gyro);
      if(accValues.isFull())
      {
        accNoises.add(Vector3<>(sqr(acc.x - accAverage.x), sqr(acc.y - accAverage.y), sqr(acc.z - accAverage.z)));
        gyroNoises.add(Vector2<>(sqr(gyro.x - gyroAverage.x), sqr(gyro.y - gyroAverage.y)));
      }
      Vector3<> accNoise = accNoises.getAverage();
      Vector2<> gyroNoise = gyroNoises.getAverage();
      PLOT("module:GroundContactDetector:accNoiseX", accNoise.x);
      PLOT("module:GroundContactDetector:accNoiseY", accNoise.y);
      PLOT("module:GroundContactDetector:accNoiseZ", accNoise.z);
      PLOT("module:GroundContactDetector:gyroNoiseX", gyroNoise.x);
      PLOT("module:GroundContactDetector:gyroNoiseY", gyroNoise.y);

      if(accNoises.isFull() &&
         accAverage.z < -5.f && std::abs(accAverage.x) < 5.f && std::abs(accAverage.y) < 5.f &&
         accNoise.x < noContactMinAccNoise && accNoise.y < noContactMinAccNoise && accNoise.z < noContactMinAccNoise &&
         gyroNoise.x < noContactMinGyroNoise && gyroNoise.y < noContactMinGyroNoise)
      {
        contact = true;
        useAngle = false;
        contactStartTime = theFrameInfo.time;
        angleNoises.clear();
        calibratedAccZValues.clear();
      }
    }
  }

  groundContactState.contact = contact;

  expectedRotationInv = theRobotModel.limbs[MassCalibration::footLeft].translation.z > theRobotModel.limbs[MassCalibration::footRight].translation.z ? theRobotModel.limbs[MassCalibration::footLeft].rotation : theRobotModel.limbs[MassCalibration::footRight].rotation;
}
コード例 #18
0
void HeadMotionEngine::update(HeadJointRequest& headJointRequest)
{
  // update requested angles
  requestedPan = theHeadAngleRequest.pan;
  requestedTilt = theHeadAngleRequest.tilt;

  //
  float maxAcc = theGroundContactState.contact ? 10.f : 1.f; // arbitrary value that seems to be good...
  MODIFY("module:HeadMotionEngine:maxAcceleration", maxAcc);

  float pan = requestedPan == JointAngles::off ? JointAngles::off : Rangef(theHeadLimits.minPan(), theHeadLimits.maxPan()).limit(requestedPan);
  float tilt = requestedTilt == JointAngles::off ? JointAngles::off : theHeadLimits.getTiltBound(pan).limit(requestedTilt);

  const float deltaTime = theFrameInfo.cycleTime;
  const Vector2f position(headJointRequest.pan == JointAngles::off ? theJointAngles.angles[Joints::headYaw] : headJointRequest.pan,
                          headJointRequest.tilt == JointAngles::off ? theJointAngles.angles[Joints::headPitch] : headJointRequest.tilt);
  const Vector2f target(pan == JointAngles::off ? 0.f : pan, tilt == JointAngles::off ? 0.f : tilt);
  Vector2f offset(target - position);
  const float distanceToTarget = offset.norm();

  // calculate max speed
  const float maxSpeedForDistance = std::sqrt(2.f * distanceToTarget * maxAcc * 0.8f);

  const float requestedSpeed = theHeadAngleRequest.stopAndGoMode
    ? theHeadAngleRequest.speed * (std::cos(pi2 / stopAndGoModeFrequenzy * theFrameInfo.time) / 2.f + .5f)
    : static_cast<float>(theHeadAngleRequest.speed);
  
  const float maxSpeed = std::min(maxSpeedForDistance, requestedSpeed);

  // max speed clipping
  if(distanceToTarget / deltaTime > maxSpeed)
    offset *= maxSpeed * deltaTime / distanceToTarget; //<=> offset.normalize(maxSpeed * deltaTime);

  // max acceleration clipping
  Vector2f speed(offset / deltaTime);
  Vector2f acc((speed - lastSpeed) / deltaTime);
  const float accSquareAbs = acc.squaredNorm();
  if(accSquareAbs > maxAcc * maxAcc)
  {
    acc *= maxAcc * deltaTime / std::sqrt(accSquareAbs);
    speed = acc + lastSpeed;
    offset = speed * deltaTime;
  }
  /* <=>
  Vector2f speed(offset / deltaTime);
  Vector2f acc((speed - lastSpeed) / deltaTime);
  if(acc.squaredNorm() > maxAcc * maxAcc)
  {
    speed = acc.normalize(maxAcc * deltaTime) + lastSpeed;
    offset = speed * deltaTime;
  }
   */
  PLOT("module:HeadMotionEngine:speed", toDegrees(speed.norm()));

  // calculate new position
  Vector2f newPosition(position + offset);

  // set new position
  headJointRequest.pan = pan == JointAngles::off ? JointAngles::off : newPosition.x();
  headJointRequest.tilt = tilt == JointAngles::off ? JointAngles::off : newPosition.y();
  headJointRequest.moving = pan != JointAngles::off && tilt != JointAngles::off && ((newPosition - position) / deltaTime).squaredNorm() > sqr(maxAcc * deltaTime * 0.5f);

  // check reachability
  headJointRequest.reachable = true;
  if(pan != requestedPan || tilt != requestedTilt)
    headJointRequest.reachable = false;

  // store some values for the next iteration
  lastSpeed = speed;
}
コード例 #19
0
bool TeamDataProvider::handleMessage(InMessage& message)
{
  /*
  The robotNumber and the three flags hasGroundContact, isUpright and isPenalized should always be updated.
  */
  switch(message.getMessageID())
  {
  case idNTPHeader:
    VERIFY(ntp.handleMessage(message));
    timeStamp = ntp.receiveTimeStamp;
    return false;
  case idNTPIdentifier:
  case idNTPRequest:
  case idNTPResponse:
    return ntp.handleMessage(message);

  case idRobot:
    message.bin >> robotNumber;
    if(robotNumber != theRobotInfo.number)
      if(robotNumber >= TeamMateData::firstPlayer && robotNumber < TeamMateData::numOfPlayers)
        theTeamMateData.timeStamps[robotNumber] = timeStamp;
    return true;

  case idReleaseOptions:
    message.bin >> Global::getReleaseOptions();
    return true;

  case idSSLVisionData:
  {
    message.bin >> theSSLVisionData;
    unsigned remoteTimestamp = theSSLVisionData.recentData.top().receiveTimestamp;
    REMOTE_TO_LOCAL_TIME(theSSLVisionData.recentData.top().receiveTimestamp);
    unsigned localTimestamp = theSSLVisionData.recentData.top().receiveTimestamp;
    int offset = (int) localTimestamp - (int) remoteTimestamp;
    PLOT("module:TeamDataProvider:sslVisionOffset", offset);
  }
  return true;

  case idGroundTruthBallModel:
  {
    Vector2<> position;
    message.bin >> theGroundTruthBallModel.timeWhenLastSeen >> position;
    REMOTE_TO_LOCAL_TIME(theGroundTruthBallModel.timeWhenLastSeen);
    if(theOwnTeamInfo.teamColor == TEAM_BLUE)
      position *= -1;
    theGroundTruthBallModel.lastPerception.setPositionAndVelocityInFieldCoordinates(
      position, Vector2<>(), theGroundTruthRobotPose);
    theGroundTruthBallModel.estimate = theGroundTruthBallModel.lastPerception;
  }
  return true;

  case idGroundTruthRobotPose:
  {
    char teamColor,
         id;
    unsigned timeStamp;
    Pose2D robotPose;
    message.bin >> teamColor >> id >> timeStamp >> robotPose;
    if(teamColor == (int)theOwnTeamInfo.teamColor && id == theRobotInfo.number)
    {
      if(theOwnTeamInfo.teamColor == TEAM_BLUE)
        robotPose = Pose2D(pi) + robotPose;
      (Pose2D&) theGroundTruthRobotPose = robotPose;
    }
  }
  return true;

  case idTeamMateIsPenalized:
    if(robotNumber != theRobotInfo.number)
      if(robotNumber >= TeamMateData::firstPlayer && robotNumber < TeamMateData::numOfPlayers)
        message.bin >> theTeamMateData.isPenalized[robotNumber];
    return true;

  case idTeamMateHasGroundContact:
    if(robotNumber != theRobotInfo.number)
      if(robotNumber >= TeamMateData::firstPlayer && robotNumber < TeamMateData::numOfPlayers)
        message.bin >> theTeamMateData.hasGroundContact[robotNumber];
    return true;

  case idTeamMateIsUpright:
    if(robotNumber != theRobotInfo.number)
      if(robotNumber >= TeamMateData::firstPlayer && robotNumber < TeamMateData::numOfPlayers)
        message.bin >> theTeamMateData.isUpright[robotNumber];
    return true;

  case idTeamMateTimeSinceLastGroundContact:
    if(robotNumber != theRobotInfo.number)
      if(robotNumber >= TeamMateData::firstPlayer && robotNumber < TeamMateData::numOfPlayers)
      {
        message.bin >> theTeamMateData.timeSinceLastGroundContact[robotNumber];
        REMOTE_TO_LOCAL_TIME(theTeamMateData.timeSinceLastGroundContact[robotNumber]);
      }
    return true;

  default:
    break;
  }
コード例 #20
0
void FootContactModelProvider::update(FootContactModel& model)
{
  MODIFY("module:FootContactModelProvider:parameters", p);

  // Check, if any bumper is pressed
  bool leftFootLeft = checkContact(KeyStates::leftFootLeft, leftFootLeftDuration);
  bool leftFootRight = checkContact(KeyStates::leftFootRight, leftFootRightDuration);
  bool rightFootLeft = checkContact(KeyStates::rightFootLeft, rightFootLeftDuration);
  bool rightFootRight = checkContact(KeyStates::rightFootRight, rightFootRightDuration);

  // Update statistics
  if (leftFootLeft || leftFootRight)
  {
    contactBufferLeft.add(1);
    contactDurationLeft++;
  }
  else
  {
    contactBufferLeft.add(0);
    contactDurationLeft = 0;
  }
  if (rightFootLeft || rightFootRight)
  {
    contactBufferRight.add(1);
    contactDurationRight++;
  }
  else
  {
    contactBufferRight.add(0);
    contactDurationRight = 0;
  }

  // Generate model
  if ((theMotionInfo.motion == MotionInfo::stand || theMotionInfo.motion == MotionInfo::walk) &&
      (theGameInfo.state == STATE_READY || theGameInfo.state == STATE_SET || theGameInfo.state == STATE_PLAYING) && //The bumper is used for configuration in initial
      (theFallDownState.state == FallDownState::upright))
  {
    if(contactBufferLeft.getSum() > p.contactThreshold)
    {
      model.contactLeft = true;
      model.contactDurationLeft = contactDurationLeft;
      model.lastContactLeft = theFrameInfo.time;
    }
    else
    {
      model.contactLeft = false;
      model.contactDurationLeft = 0;
    }
    if(contactBufferRight.getSum() > p.contactThreshold)
    {
      model.contactRight = true;
      model.contactDurationRight = contactDurationRight;
      model.lastContactRight = theFrameInfo.time;
    }
    else
    {
      model.contactRight = false;
      model.contactDurationRight = 0;
    }
  }
  else
  {
    model.contactLeft = false;
    model.contactRight = false;
    model.contactDurationLeft = 0;
    model.contactDurationRight = 0;
  }

  // Debugging stuff:

  if(p.debug && theFrameInfo.getTimeSince(lastSoundTime) > (int)p.soundDelay &&
    (model.contactLeft || model.contactRight))
  {
    lastSoundTime = theFrameInfo.time;
    SoundPlayer::play("doh.wav");
  }


  DECLARE_PLOT("module:FootContactModelProvider:sumLeft");
  DECLARE_PLOT("module:FootContactModelProvider:durationLeft");
  DECLARE_PLOT("module:FootContactModelProvider:contactLeft");
  DECLARE_PLOT("module:FootContactModelProvider:sumRight");
  DECLARE_PLOT("module:FootContactModelProvider:durationRight");
  DECLARE_PLOT("module:FootContactModelProvider:contactRight");
  DECLARE_PLOT("module:FootContactModelProvider:leftFootLeft");
  DECLARE_PLOT("module:FootContactModelProvider:leftFootRight");
  DECLARE_PLOT("module:FootContactModelProvider:rightFootLeft");
  DECLARE_PLOT("module:FootContactModelProvider:rightFootRight");
  PLOT("module:FootContactModelProvider:sumLeft", contactBufferLeft.getSum());
  PLOT("module:FootContactModelProvider:durationLeft", contactDurationLeft);
  PLOT("module:FootContactModelProvider:sumRight", contactBufferRight.getSum());
  PLOT("module:FootContactModelProvider:durationRight", contactDurationRight);
  PLOT("module:FootContactModelProvider:contactLeft", model.contactLeft ? 10 : 0);
  PLOT("module:FootContactModelProvider:contactRight", model.contactRight ? 10 : 0);
  PLOT("module:FootContactModelProvider:leftFootLeft", leftFootLeft ? 10 : 0);
  PLOT("module:FootContactModelProvider:leftFootRight", leftFootRight ? 10 : 0);
  PLOT("module:FootContactModelProvider:rightFootLeft", rightFootLeft ? 10 : 0);
  PLOT("module:FootContactModelProvider:rightFootRight", rightFootRight ? 10 : 0);
}
コード例 #21
0
ファイル: SensorFilter.cpp プロジェクト: Nani90/nao-robots
void SensorFilter::update(FilteredSensorData& filteredSensorData,
        const InertiaSensorData& theInertiaSensorData,
        const SensorData& theSensorData,
        const OrientationData& theOrientationData)
{
  // copy sensor data (except gyro and acc)
  Vector2BH<> gyro(filteredSensorData.data[SensorData::gyroX], filteredSensorData.data[SensorData::gyroY]);
  Vector3BH<> acc(filteredSensorData.data[SensorData::accX], filteredSensorData.data[SensorData::accY], filteredSensorData.data[SensorData::accZ]);
  (SensorData&)filteredSensorData = theSensorData;
  filteredSensorData.data[SensorData::gyroX] = gyro.x;
  filteredSensorData.data[SensorData::gyroY] = gyro.y;
  filteredSensorData.data[SensorData::accX] = acc.x;
  filteredSensorData.data[SensorData::accY] = acc.y;
  filteredSensorData.data[SensorData::accZ] = acc.z;

  // take calibrated inertia sensor data
  for(int i = 0; i < 2; ++i)
  {
    if(theInertiaSensorData.gyro[i] != InertiaSensorData::off)
      filteredSensorData.data[SensorData::gyroX + i] = theInertiaSensorData.gyro[i];
    else if(filteredSensorData.data[SensorData::gyroX + i] == SensorData::off)
      filteredSensorData.data[SensorData::gyroX + i] = 0.f;
  }
  filteredSensorData.data[SensorData::gyroZ] = 0.f;
  for(int i = 0; i < 3; ++i)
  {
    if(theInertiaSensorData.acc[i] != InertiaSensorData::off)
      filteredSensorData.data[SensorData::accX + i] = theInertiaSensorData.acc[i] / 9.80665f;
    else if(filteredSensorData.data[SensorData::accX + i] == SensorData::off)
      filteredSensorData.data[SensorData::accX + i] = 0.f;
  }

  // take orientation data
  filteredSensorData.data[SensorData::angleX] = theOrientationData.orientation.x;
  filteredSensorData.data[SensorData::angleY] = theOrientationData.orientation.y;

#ifndef RELEASE
  if(filteredSensorData.data[SensorData::gyroX] != SensorData::off)
  {
    gyroAngleXSum += filteredSensorData.data[SensorData::gyroX] * (theSensorData.timeStamp - lastIteration) * 0.001f;
    gyroAngleXSum = normalizeBH(gyroAngleXSum);
    lastIteration = theSensorData.timeStamp;
  }
  PLOT("module:SensorFilter:gyroAngleXSum", gyroAngleXSum);
#endif

  //
//  PLOT("module:SensorFilter:rawAngleX", theSensorData.data[SensorData::angleX]);
//  PLOT("module:SensorFilter:rawAngleY", theSensorData.data[SensorData::angleY]);
//
//  PLOT("module:SensorFilter:rawAccX", theSensorData.data[SensorData::accX]);
//  PLOT("module:SensorFilter:rawAccY", theSensorData.data[SensorData::accY]);
//  PLOT("module:SensorFilter:rawAccZ", theSensorData.data[SensorData::accZ]);
//
//  PLOT("module:SensorFilter:rawGyroX", theSensorData.data[SensorData::gyroX]);
//  PLOT("module:SensorFilter:rawGyroY", theSensorData.data[SensorData::gyroY]);
//  PLOT("module:SensorFilter:rawGyroZ", theSensorData.data[SensorData::gyroZ]);
//
//  PLOT("module:SensorFilter:angleX", filteredSensorData.data[SensorData::angleX]);
//  PLOT("module:SensorFilter:angleY", filteredSensorData.data[SensorData::angleY]);
//
//  PLOT("module:SensorFilter:accX", filteredSensorData.data[SensorData::accX]);
//  PLOT("module:SensorFilter:accY", filteredSensorData.data[SensorData::accY]);
//  PLOT("module:SensorFilter:accZ", filteredSensorData.data[SensorData::accZ]);
//
//  PLOT("module:SensorFilter:gyroX", filteredSensorData.data[SensorData::gyroX] != float(SensorData::off) ? filteredSensorData.data[SensorData::gyroX] : 0);
//  PLOT("module:SensorFilter:gyroY", filteredSensorData.data[SensorData::gyroY] != float(SensorData::off) ? filteredSensorData.data[SensorData::gyroY] : 0);
//  PLOT("module:SensorFilter:gyroZ", filteredSensorData.data[SensorData::gyroZ] != float(SensorData::off) ? filteredSensorData.data[SensorData::gyroZ] : 0);

  //PLOT("module:SensorFilter:us", filteredSensorData.data[SensorData::us]);
}
コード例 #22
0
ファイル: main.c プロジェクト: antoneri/5FY033
/*
 * Extracting information from a noisy signal
 */
void extract_noisy_signal(int count, double *values) {
    plot_t p;

    p.outfile = "output5.png";
    p.title = "Noisy signal";
    p.xlabel = "t [s]";
    p.ylabel = "u(t) [m]";
    p.xmin = 0;
    p.xmax = 0;
    init_gnuplot(&p);

    gsl_complex_packed_array data = calloc(2*count, sizeof(double));

    // Noisy signal
    BEGIN_SCATTER(p, "u(t)");

    double t = 0;

    for (int i = 0; i < count; i++) {
        PLOT(p, t, values[i]);
        data[2*i] = values[i];
        t += 1.0/count;
    }

    END_PLOT(p);

    p.outfile = "output6.png";
    p.title = "FFT of noisy signal";
    p.xlabel = "f [Hz]";
    p.ylabel = "U(f) [m]";
    p.xmin = -1500;
    p.xmax = 1500;
    init_gnuplot(&p);

    gsl_fft_complex_radix2_forward(data, 1, count);

    // FFT of noisy signal
    BEGIN_SCATTER(p, "U(f)");

    double f = -count/2;

    for (int i = count; i < 2*count; i += 2) {
        double y = data[i];
        data[i] = H(f) * data[i];
        data[i+1] = H(f) * data[i+1];
        PLOT(p, f, y);
        f += 1.0;
    }

    for (int i = 0; i < count; i += 2) {
        double y = data[i];
        data[i] = H(f) * data[i];
        data[i+1] = H(f) * data[i+1];
        PLOT(p, f, y);
        f += 1.0;
    }

    END_PLOT(p);

    gsl_fft_complex_radix2_inverse(data, 1, count);

    p.outfile = "output7.png";
    p.title = "Inverse FFT of data";
    p.xlabel = "t [s]";
    p.ylabel = "u(t) [m]";
    p.xmin = 0;
    p.xmax = 1;
    init_gnuplot(&p);

    // Filtered signal
    BEGIN_PLOT(p, "u(t)");

    t = 0;

    double umax = 0;
    int periods = 0;
    char letter = 0;
    int bits = 0;
    const double plen = 0.0078;

    for (int i = 0; i < 2*count; i += 2) {
        if (t >= plen * (double)periods && t < plen * ((double)periods + 1.0)) {
            if (data[i] > umax) umax = data[i];
        } else {
            if (umax < 1) letter *= 2;
            else letter = letter*2 + 1;
            bits++;
            if (bits % 8 == 0) {
                printf("%c", letter);
                bits = 0;
                letter = 0;
            }
            umax = 0;
            periods++;
        }

        PLOT(p, t, (data[i]));
        t += 1.0/count;
    }
    if (umax < 1) letter *= 2;
    else letter = letter*2 + 1;
    printf("%c\n", letter);

    END_PLOT(p);
}
コード例 #23
0
void TorsoMatrixProvider::update(TorsoMatrixBH& torsoMatrix)
{
  // remove the z-rotation from the orientation data rotation matrix
  Vector3BH<> g(theOrientationDataBH.rotation.c1.z, -theOrientationDataBH.rotation.c0.z, 0.f);
  float w = std::atan2(std::sqrt(g.x * g.x + g.y * g.y), theOrientationDataBH.rotation.c2.z);
  RotationMatrixBH torsoRotation(g, w);

  // calculate "center of hip" position from left foot
  Pose3DBH fromLeftFoot(torsoRotation);
  fromLeftFoot.conc(theRobotModelBH.limbs[MassCalibrationBH::footLeft]);
  fromLeftFoot.translate(0, 0, -theRobotDimensionsBH.heightLeg5Joint);
  fromLeftFoot.translation *= -1.;
  fromLeftFoot.rotation = torsoRotation;

  // calculate "center of hip" position from right foot
  Pose3DBH fromRightFoot(torsoRotation);
  fromRightFoot.conc(theRobotModelBH.limbs[MassCalibrationBH::footRight]);
  fromRightFoot.translate(0, 0, -theRobotDimensionsBH.heightLeg5Joint);
  fromRightFoot.translation *= -1.;
  fromRightFoot.rotation = torsoRotation;

  // get foot z-rotations
  const Pose3DBH& leftFootInverse(theRobotModelBH.limbs[MassCalibrationBH::footLeft].invert());
  const Pose3DBH& rightFootInverse(theRobotModelBH.limbs[MassCalibrationBH::footRight].invert());
  const float leftFootZRotation = leftFootInverse.rotation.getZAngle();
  const float rightFootZRotation = rightFootInverse.rotation.getZAngle();

  // determine used foot
  const bool useLeft = fromLeftFoot.translation.z > fromRightFoot.translation.z;
  torsoMatrix.leftSupportFoot = useLeft;

  // calculate foot span
  const Vector3BH<> newFootSpan(fromRightFoot.translation - fromLeftFoot.translation);

  // and construct the matrix
  Pose3DBH newTorsoMatrix;
  newTorsoMatrix.translate(newFootSpan.x / (useLeft ? 2.f : -2.f), newFootSpan.y / (useLeft ? 2.f : -2.f), 0);
  newTorsoMatrix.conc(useLeft ? fromLeftFoot : fromRightFoot);

  // calculate torso offset
  if(torsoMatrix.translation.z != 0) // the last torso matrix should be valid
  {
    Pose3DBH& torsoOffset(torsoMatrix.offset);
    torsoOffset = torsoMatrix.invert();
    torsoOffset.translate(lastFootSpan.x / (useLeft ? 2.f : -2.f), lastFootSpan.y / (useLeft ? 2.f : -2.f), 0);
    torsoOffset.rotateZ(useLeft ? float(leftFootZRotation - lastLeftFootZRotation) : float(rightFootZRotation - lastRightFootZRotation));
    torsoOffset.translate(newFootSpan.x / (useLeft ? -2.f : 2.f), newFootSpan.y / (useLeft ? -2.f : 2.f), 0);
    torsoOffset.conc(newTorsoMatrix);
  }

  // adopt new matrix and footSpan
  (Pose3DBH&)torsoMatrix = newTorsoMatrix;
  lastLeftFootZRotation = leftFootZRotation;
  lastRightFootZRotation = rightFootZRotation;
  lastFootSpan = newFootSpan;

  // valid?
  torsoMatrix.isValid = theGroundContactStateBH.contact;

  //
  PLOT("module:TorsoMatrixProvider:footSpanX", newFootSpan.x);
  PLOT("module:TorsoMatrixProvider:footSpanY", newFootSpan.y);
  PLOT("module:TorsoMatrixProvider:footSpanZ", newFootSpan.z);

  PLOT("module:TorsoMatrixProvider:torsoMatrixX", torsoMatrix.translation.x);
  PLOT("module:TorsoMatrixProvider:torsoMatrixY", torsoMatrix.translation.y);
  PLOT("module:TorsoMatrixProvider:torsoMatrixZ", torsoMatrix.translation.z);
}
コード例 #24
0
void MotionSelector::update(MotionSelection& motionSelection)
{
  static int interpolationTimes[MotionRequest::numOfMotions];
  interpolationTimes[MotionRequest::walk] = 790;
  interpolationTimes[MotionRequest::kick] = 200;
  interpolationTimes[MotionRequest::specialAction] = 200;
  interpolationTimes[MotionRequest::stand] = 600;
  interpolationTimes[MotionRequest::getUp] = 600;
  static const int playDeadDelay(2000);

  if(lastExecution)
  {
    MotionRequest::Motion requestedMotion = theMotionRequest.motion;
    if(theMotionRequest.motion == MotionRequest::walk && !theGroundContactState.contact)
      requestedMotion = MotionRequest::stand;

    if(forceStand && (lastMotion == MotionRequest::walk || lastMotion == MotionRequest::stand))
    {
      requestedMotion = MotionRequest::stand;
      forceStand = false;
    }

    // check if the target motion can be the requested motion (mainly if leaving is possible)
    if((lastMotion == MotionRequest::walk && (!&theWalkingEngineOutput || theWalkingEngineOutput.isLeavingPossible || !theGroundContactState.contact)) ||
       lastMotion == MotionRequest::stand || // stand can always be left
       (lastMotion == MotionRequest::specialAction && theSpecialActionsOutput.isLeavingPossible) ||
       (lastMotion == MotionRequest::kick && theKickEngineOutput.isLeavingPossible) ||
       (lastMotion == MotionRequest::getUp && theGetUpEngineOutput.isLeavingPossible)) //never immediatly leave kick or get up
    {
      motionSelection.targetMotion = requestedMotion;
    }

    if(requestedMotion == MotionRequest::specialAction)
    {
      motionSelection.specialActionRequest = theMotionRequest.specialActionRequest;
    }
    else
    {
      motionSelection.specialActionRequest = SpecialActionRequest();
      if(motionSelection.targetMotion == MotionRequest::specialAction)
        motionSelection.specialActionRequest.specialAction = SpecialActionRequest::numOfSpecialActionIDs;
    }

    // increase / decrease all ratios according to target motion
    const unsigned deltaTime(theFrameInfo.getTimeSince(lastExecution));
    const int interpolationTime = prevMotion == MotionRequest::specialAction && lastActiveSpecialAction == SpecialActionRequest::playDead ? playDeadDelay : interpolationTimes[motionSelection.targetMotion];
    float delta((float)deltaTime / interpolationTime);
    ASSERT(SystemCall::getMode() == SystemCall::logfileReplay || delta > 0.00001f);
    float sum(0);
    for(int i = 0; i < MotionRequest::numOfMotions; i++)
    {
      if(i == motionSelection.targetMotion)
        motionSelection.ratios[i] += delta;
      else
        motionSelection.ratios[i] -= delta;
      motionSelection.ratios[i] = std::max(motionSelection.ratios[i], 0.0f); // clip ratios
      sum += motionSelection.ratios[i];
    }
    ASSERT(sum != 0);
    // normalize ratios
    for(int i = 0; i < MotionRequest::numOfMotions; i++)
    {
      motionSelection.ratios[i] /= sum;
      if(std::abs(motionSelection.ratios[i] - 1.f) < 0.00001f)
        motionSelection.ratios[i] = 1.f; // this should fix a "motionSelection.ratios[motionSelection.targetMotion] remains smaller than 1.f" bug
    }

    if(motionSelection.ratios[MotionRequest::specialAction] < 1.f)
    {
      if(motionSelection.targetMotion == MotionRequest::specialAction)
        motionSelection.specialActionMode = MotionSelection::first;
      else
        motionSelection.specialActionMode = MotionSelection::deactive;
    }
    else
      motionSelection.specialActionMode = MotionSelection::active;

    if(motionSelection.specialActionMode == MotionSelection::active && motionSelection.specialActionRequest.specialAction != SpecialActionRequest::numOfSpecialActionIDs)
      lastActiveSpecialAction = motionSelection.specialActionRequest.specialAction;
  }
  lastExecution = theFrameInfo.time;
  if(lastMotion != motionSelection.targetMotion)
    prevMotion = lastMotion;
  lastMotion = motionSelection.targetMotion;

  PLOT("module:MotionSelector:ratios:walk", motionSelection.ratios[MotionRequest::walk]);
  PLOT("module:MotionSelector:ratios:stand", motionSelection.ratios[MotionRequest::stand]);
  PLOT("module:MotionSelector:ratios:specialAction", motionSelection.ratios[MotionRequest::specialAction]);
  PLOT("module:MotionSelector:lastMotion", lastMotion);
  PLOT("module:MotionSelector:prevMotion", prevMotion);
  PLOT("module:MotionSelector:targetMotion", motionSelection.targetMotion);
}
コード例 #25
0
ファイル: main.c プロジェクト: antoneri/5FY033
/*
 * The Fourier transform of a Gaussian
 */
void gaussian(void) {
    plot_t p;

    p.outfile = "output1.png";
    p.title = "Gaussian";
    p.xlabel = "t [s]";
    p.ylabel = "x(t) [m]";
    p.xmin = -N/2*dt;
    p.xmax = N/2*dt;
    init_gnuplot(&p);

    gsl_complex_packed_array data = calloc(2*N, sizeof(double));

    BEGIN_SCATTER(p, "x(t)");

    double t = -N/2 * dt;

    for (int i = 0; i < 2*N; i += 2) {
        data[i] = x(t);
        PLOT(p, t, data[i]);
        t += dt;
    }

    END_PLOT(p);

    gsl_fft_complex_radix2_forward(data, 1, N);

    p.outfile = "output2.png";
    p.title = "FFT of a Gauissian";
    p.xlabel = "f [Hz]";
    p.ylabel = "X(f) [m]";
    p.xmin = -512;
    p.xmax = 512;
    init_gnuplot(&p);

    fprintf(p.fp, "plot '-' title 'Real',"
            "'-' title 'Imag',"
            "'-' with lines title 'Analytical'\n");

    // Real part
    double f = -1.0/(2.0*dt);

    for (int i = N; i < 2*N; i += 2) {
        PLOT(p, f, creal(shift(f))*(data[i]/(double) N));
        f += 1.0;
    }

    for (int i = 0; i < N; i += 2) {
        PLOT(p, f, creal(shift(f))*(data[i]/(double) N));
        f += 1.0;
    }

    // Imag. part
    NEW_PLOT(p);

    f = -1.0/(2.0*dt);

    for (int i = N+1; i <= 2*N; i += 2) {
        PLOT(p, f, cimag(shift(f))*data[i]/(double) N);
        f += 1.0;
    }

    for (int i = 1; i <= N; i += 2) {
        PLOT(p, f, cimag(shift(f))*data[i]/(double) N);
        f += 1.0;
    }

    // Analytical
    NEW_PLOT(p);

    f = -1.0/(2.0*dt);

    for (int i = 1; i <= 2*N; i += 2) {
        PLOT(p, f, X(f));
        f += 1.0;
    }

    END_PLOT(p);

    free(data);
}
コード例 #26
0
void MotionCombinator::update(JointRequest& jointRequest)
{
  specialActionOdometry += theSpecialActionsOutput.odometryOffset;

  copy(theLegJointRequest, jointRequest, theStiffnessSettings, Joints::headYaw, Joints::headPitch);

  copy(theArmJointRequest, jointRequest, theStiffnessSettings, Joints::firstArmJoint, Joints::rHand);
  copy(theLegJointRequest, jointRequest, theStiffnessSettings, Joints::firstLegJoint, Joints::rAnkleRoll);

  ASSERT(jointRequest.isValid());

  // Find fully active motion and set MotionInfo
  if(theLegMotionSelection.ratios[theLegMotionSelection.targetMotion] == 1.f)
  {
    // default values
    motionInfo.motion = theLegMotionSelection.targetMotion;
    motionInfo.isMotionStable = true;
    motionInfo.upcomingOdometryOffset = Pose2f();

    Pose2f odometryOffset;
    switch(theLegMotionSelection.targetMotion)
    {
      case MotionRequest::walk:
        odometryOffset = theWalkingEngineOutput.odometryOffset;
        motionInfo.walkRequest = theWalkingEngineOutput.executedWalk;
        motionInfo.upcomingOdometryOffset = theWalkingEngineOutput.upcomingOdometryOffset;
        break;
      case MotionRequest::kick:
        odometryOffset = theKickEngineOutput.odometryOffset;
        motionInfo.kickRequest = theKickEngineOutput.executedKickRequest;
        motionInfo.isMotionStable = theKickEngineOutput.isStable;
        break;
      case MotionRequest::specialAction:
        odometryOffset = specialActionOdometry;
        specialActionOdometry = Pose2f();
        motionInfo.specialActionRequest = theSpecialActionsOutput.executedSpecialAction;
        motionInfo.isMotionStable = theSpecialActionsOutput.isMotionStable;
        break;
      case MotionRequest::getUp:
        motionInfo.isMotionStable = false;
        odometryOffset = theGetUpEngineOutput.odometryOffset;
        break;
      case MotionRequest::stand:
      default:
        break;
    }

    if(theRobotInfo.hasFeature(RobotInfo::zGyro) && (theFallDownState.state == FallDownState::upright || theLegMotionSelection.targetMotion == MotionRequest::getUp))
    {
      Vector3f rotatedGyros = theInertialData.orientation * theInertialData.gyro.cast<float>();
      odometryOffset.rotation = rotatedGyros.z() * theFrameInfo.cycleTime;
    }

    odometryData += odometryOffset;
  }

  if(emergencyOffEnabled && motionInfo.motion != MotionRequest::getUp)
  {
    if(theFallDownState.state == FallDownState::falling && motionInfo.motion != MotionRequest::specialAction)

    {
      safeFall(jointRequest);
      centerHead(jointRequest);
      currentRecoveryTime = 0;

      ASSERT(jointRequest.isValid());
    }
    else if(theFallDownState.state == FallDownState::onGround && motionInfo.motion != MotionRequest::specialAction)
    {
      centerHead(jointRequest);
    }
    else
    {
      if(theFallDownState.state == FallDownState::upright)
      {
        headYawInSafePosition = false;
        headPitchInSafePosition = false;
      }

      if(currentRecoveryTime < recoveryTime)
      {
        currentRecoveryTime += 1;
        float ratio = (1.f / float(recoveryTime)) * currentRecoveryTime;
        for(int i = 0; i < Joints::numOfJoints; i++)
        {
          jointRequest.stiffnessData.stiffnesses[i] = 30 + int(ratio * float(jointRequest.stiffnessData.stiffnesses[i] - 30));
        }
      }
    }
  }

  if(debugArms)
    debugReleaseArms(jointRequest);
  eraseStiffness(jointRequest);

  float sum = 0;
  int count = 0;
  for(int i = Joints::lHipYawPitch; i < Joints::numOfJoints; i++)
  {
    if(jointRequest.angles[i] != JointAngles::off && jointRequest.angles[i] != JointAngles::ignore &&
       lastJointRequest.angles[i] != JointAngles::off && lastJointRequest.angles[i] != JointAngles::ignore)
    {
      sum += std::abs(jointRequest.angles[i] - lastJointRequest.angles[i]);
      count++;
    }
  }
  PLOT("module:MotionCombinator:deviations:JointRequest:legsOnly", sum / count);
  for(int i = 0; i < Joints::lHipYawPitch; i++)
  {
    if(jointRequest.angles[i] != JointAngles::off && jointRequest.angles[i] != JointAngles::ignore &&
       lastJointRequest.angles[i] != JointAngles::off && lastJointRequest.angles[i] != JointAngles::ignore)
    {
      sum += std::abs(jointRequest.angles[i] - lastJointRequest.angles[i]);
      count++;
    }
  }
  PLOT("module:MotionCombinator:deviations:JointRequest:all", sum / count);

  sum = 0;
  count = 0;
  for(int i = Joints::lHipYawPitch; i < Joints::numOfJoints; i++)
  {
    if(lastJointRequest.angles[i] != JointAngles::off && lastJointRequest.angles[i] != JointAngles::ignore)
    {
      sum += std::abs(lastJointRequest.angles[i] - theJointAngles.angles[i]);
      count++;
    }
  }
  PLOT("module:MotionCombinator:differenceToJointData:legsOnly", sum / count);

  for(int i = 0; i < Joints::lHipYawPitch; i++)
  {
    if(lastJointRequest.angles[i] != JointAngles::off && lastJointRequest.angles[i] != JointAngles::ignore)
    {
      sum += std::abs(lastJointRequest.angles[i] - theJointAngles.angles[i]);
      count++;
    }
  }
  lastJointRequest = jointRequest;
  PLOT("module:MotionCombinator:differenceToJointData:all", sum / count);

#ifndef NDEBUG
  if(!jointRequest.isValid())
  {
    {
      std::string logDir = "";
#ifdef TARGET_ROBOT
      logDir = "../logs/";
#endif
      OutMapFile stream(logDir + "jointRequest.log");
      stream << jointRequest;
      OutMapFile stream2(logDir + "motionSelection.log");
      stream2 << theLegMotionSelection;
    }
    ASSERT(false);
  }
#endif
}
コード例 #27
0
void ArmContactModelProvider::update(ArmContactModel& model)
{
  MODIFY("module:ArmContactModelProvider:parameters", p);
  DECLARE_PLOT("module:ArmContactModelProvider:errorLeftX");
  DECLARE_PLOT("module:ArmContactModelProvider:errorRightX");
  DECLARE_PLOT("module:ArmContactModelProvider:errorLeftY");
  DECLARE_PLOT("module:ArmContactModelProvider:errorRightY");
  DECLARE_PLOT("module:ArmContactModelProvider:errorDurationLeft");
  DECLARE_PLOT("module:ArmContactModelProvider:errorDurationRight");
  DECLARE_PLOT("module:ArmContactModelProvider:errorYThreshold");
  DECLARE_PLOT("module:ArmContactModelProvider:errorXThreshold");
  DECLARE_PLOT("module:ArmContactModelProvider:contactLeft");
  DECLARE_PLOT("module:ArmContactModelProvider:contactRight");

  DECLARE_DEBUG_DRAWING("module:ArmContactModelProvider:armContact", "drawingOnField");

  CIRCLE("module:ArmContactModelProvider:armContact", 0, 0, 200, 30, Drawings::ps_solid, ColorClasses::blue, Drawings::ps_null, ColorClasses::blue);
  CIRCLE("module:ArmContactModelProvider:armContact", 0, 0, 400, 30, Drawings::ps_solid, ColorClasses::blue, Drawings::ps_null, ColorClasses::blue);
  CIRCLE("module:ArmContactModelProvider:armContact", 0, 0, 600, 30, Drawings::ps_solid, ColorClasses::blue, Drawings::ps_null, ColorClasses::blue);
  CIRCLE("module:ArmContactModelProvider:armContact", 0, 0, 800, 30, Drawings::ps_solid, ColorClasses::blue, Drawings::ps_null, ColorClasses::blue);
  CIRCLE("module:ArmContactModelProvider:armContact", 0, 0, 1000, 30, Drawings::ps_solid, ColorClasses::blue, Drawings::ps_null, ColorClasses::blue);
  CIRCLE("module:ArmContactModelProvider:armContact", 0, 0, 1200, 30, Drawings::ps_solid, ColorClasses::blue, Drawings::ps_null, ColorClasses::blue);


  /* Buffer arm angles */
  struct ArmAngles angles;
  angles.leftX = theJointRequest.angles[JointData::LShoulderPitch];
  angles.leftY = theJointRequest.angles[JointData::LShoulderRoll];
  angles.rightX = theJointRequest.angles[JointData::RShoulderPitch];
  angles.rightY = theJointRequest.angles[JointData::RShoulderRoll];
  angleBuffer.add(angles);
  
  /* Reset in case of a fall or penalty */
  if(theFallDownState.state == FallDownState::onGround || theRobotInfo.penalty != PENALTY_NONE)
  {
    leftErrorBuffer.init();
    rightErrorBuffer.init();
  }

  Pose2D odometryOffset = theOdometryData - lastOdometry;
  lastOdometry = theOdometryData;
  
  const Vector3<>& leftHandPos3D = theRobotModel.limbs[MassCalibration::foreArmLeft].translation;
  Vector2<> leftHandPos(leftHandPos3D.x, leftHandPos3D.y);
  Vector2<> leftHandSpeed = (odometryOffset + Pose2D(leftHandPos) - Pose2D(lastLeftHandPos)).translation / theFrameInfo.cycleTime;
  float leftFactor = std::max(0.f, 1.f - leftHandSpeed.abs() / p.speedBasedErrorReduction);
  lastLeftHandPos = leftHandPos;

  const Vector3<>& rightHandPos3D = theRobotModel.limbs[MassCalibration::foreArmRight].translation;
  Vector2<> rightHandPos(rightHandPos3D.x, rightHandPos3D.y);
  Vector2<> rightHandSpeed = (odometryOffset + Pose2D(rightHandPos) - Pose2D(lastRightHandPos)).translation / theFrameInfo.cycleTime;
  float rightFactor = std::max(0.f, 1.f - rightHandSpeed.abs() / p.speedBasedErrorReduction);
  lastRightHandPos = rightHandPos;
  
  /* Check for arm contact */
  // motion types to take into account: stand, walk (if the robot is upright)
  if((theMotionInfo.motion == MotionInfo::stand || theMotionInfo.motion == MotionInfo::walk) &&
     (theFallDownState.state == FallDownState::upright || theFallDownState.state == FallDownState::staggering) &&
     (theGameInfo.state == STATE_PLAYING || theGameInfo.state == STATE_READY) &&
     (theRobotInfo.penalty == PENALTY_NONE)) // TICKET 897: ArmContact only if robot is not penalized
  {
    checkArm(LEFT, leftFactor);
    checkArm(RIGHT, rightFactor);

    //left and right are projections of the 3 dimensional shoulder-joint vector
    //onto the x-y plane.
    Vector2f left = leftErrorBuffer.getAverageFloat();
    Vector2f right = rightErrorBuffer.getAverageFloat();

    
    //Determine if we are being pushed or not
    bool leftX  = fabs(left.x) > fromDegrees(p.errorXThreshold);
    bool leftY  = fabs(left.y) > fromDegrees(p.errorYThreshold);
    bool rightX = fabs(right.x)> fromDegrees(p.errorXThreshold);
    bool rightY = fabs(right.y)> fromDegrees(p.errorYThreshold);

    // update the model
    model.contactLeft  = leftX || leftY;
    model.contactRight = rightX || rightY;

    // The duration of the contact is counted upwards as long as the error
    //remains. Otherwise it is reseted to 0.
    model.durationLeft  = model.contactLeft  ? model.durationLeft + 1 : 0;
    model.durationRight = model.contactRight ? model.durationRight + 1 : 0;

    model.contactLeft &= model.durationLeft < p.malfunctionThreshold;
    model.contactRight &= model.durationRight < p.malfunctionThreshold;


    if(model.contactLeft)
    {
      model.timeOfLastContactLeft = theFrameInfo.time;
    }
    if(model.contactRight)
    {
      model.timeOfLastContactRight = theFrameInfo.time;
    }
    
    model.pushDirectionLeft = getDirection(LEFT, leftX, leftY, left);
    model.pushDirectionRight = getDirection(RIGHT, rightX, rightY, right);

    model.lastPushDirectionLeft = model.pushDirectionLeft != ArmContactModel::NONE ? model.pushDirectionLeft : model.lastPushDirectionLeft;
    model.lastPushDirectionRight = model.pushDirectionRight != ArmContactModel::NONE ? model.pushDirectionRight : model.lastPushDirectionRight;

    PLOT("module:ArmContactModelProvider:errorLeftX",         toDegrees(left.x));
    PLOT("module:ArmContactModelProvider:errorRightX",        toDegrees(right.x));
    PLOT("module:ArmContactModelProvider:errorLeftY",         toDegrees(left.y));
    PLOT("module:ArmContactModelProvider:errorRightY",        toDegrees(right.y));
    PLOT("module:ArmContactModelProvider:errorDurationLeft",  model.durationLeft);
    PLOT("module:ArmContactModelProvider:errorDurationRight", model.durationRight);
    PLOT("module:ArmContactModelProvider:errorYThreshold",    toDegrees(p.errorYThreshold));
    PLOT("module:ArmContactModelProvider:errorXThreshold",    toDegrees(p.errorXThreshold));
    PLOT("module:ArmContactModelProvider:contactLeft",        model.contactLeft ? 10.0 : 0.0);
    PLOT("module:ArmContactModelProvider:contactRight",       model.contactRight ? 10.0 : 0.0);

    ARROW("module:ArmContactModelProvider:armContact", 0, 0, -(toDegrees(left.y) * SCALE), toDegrees(left.x) * SCALE, 20, Drawings::ps_solid, ColorClasses::green);
    ARROW("module:ArmContactModelProvider:armContact", 0, 0, toDegrees(right.y) * SCALE, toDegrees(right.x) * SCALE, 20, Drawings::ps_solid, ColorClasses::red);

    COMPLEX_DRAWING("module:ArmContactModelProvider:armContact",
    {
      DRAWTEXT("module:ArmContactModelProvider:armContact", -2300, 1300, 20, ColorClasses::black, "LEFT");
      DRAWTEXT("module:ArmContactModelProvider:armContact", -2300, 1100, 20, ColorClasses::black, "ErrorX: " << toDegrees(left.x));
      DRAWTEXT("module:ArmContactModelProvider:armContact", -2300, 900, 20, ColorClasses::black,  "ErrorY: " << toDegrees(left.y));
      DRAWTEXT("module:ArmContactModelProvider:armContact", -2300, 500, 20, ColorClasses::black,  ArmContactModel::getName(model.pushDirectionLeft));
      DRAWTEXT("module:ArmContactModelProvider:armContact", -2300, 300, 20, ColorClasses::black,  "Time: " << model.timeOfLastContactLeft);

      DRAWTEXT("module:ArmContactModelProvider:armContact", 1300, 1300, 20, ColorClasses::black, "RIGHT");
      DRAWTEXT("module:ArmContactModelProvider:armContact", 1300, 1100, 20, ColorClasses::black, "ErrorX: " << toDegrees(right.x));
      DRAWTEXT("module:ArmContactModelProvider:armContact", 1300, 900, 20, ColorClasses::black,  "ErrorY: " << toDegrees(right.y));
      DRAWTEXT("module:ArmContactModelProvider:armContact", 1300, 500, 20, ColorClasses::black,  ArmContactModel::getName(model.pushDirectionRight));
      DRAWTEXT("module:ArmContactModelProvider:armContact", 1300, 300, 20, ColorClasses::black,  "Time: " << model.timeOfLastContactRight);

      if (model.contactLeft)
      {
        CROSS("module:ArmContactModelProvider:armContact", -2000, 0, 100, 20, Drawings::ps_solid, ColorClasses::red);
      }
      if (model.contactRight)
      {
        CROSS("module:ArmContactModelProvider:armContact", 2000, 0, 100, 20, Drawings::ps_solid, ColorClasses::red);
      }
  });
コード例 #28
0
void GroundContactDetector::update(GroundContactState& groundContactState)
{
  MODIFY("module:GroundContactDetector:parameters", p);
  PLOT("module:GroundContactDetector:groundContact", groundContactState.contact ? 0.75 : 0.25);

#ifdef TARGET_ROBOT
  if(p.forceContact)
#endif
  {
    groundContactState.contact = true;
    //groundContactState.contactSafe = true;
    return;
  }

  // states
  ContactState stateFsrLeft = checkFsr(true);
  ContactState stateFsrRight = checkFsr(false);
  ContactState stateLoad = checkLoad();
  // contact plots
  PLOT("module:GroundContactDetector:contactLoad", stateLoad.contact ? 0.75 : 0.25);
  PLOT("module:GroundContactDetector:contactFsrLeft", stateFsrLeft.contact ? 0.75 : 0.25);
  PLOT("module:GroundContactDetector:contactFsrRight", stateFsrRight.contact ? 0.75 : 0.25);
  // confidence plots
  PLOT("module:GroundContactDetector:confidenceLoad", stateLoad.confidence);
  PLOT("module:GroundContactDetector:confidenceFsrLeft", stateFsrLeft.confidence);
  PLOT("module:GroundContactDetector:confidenceFsrRight", stateFsrRight.confidence);

  float confidenceContact = 0.f;
  float confidenceNoContact = 0.f;
  if(stateFsrLeft.contact) confidenceContact += stateFsrLeft.confidence;
  else confidenceNoContact += stateFsrLeft.confidence;
  if(stateFsrRight.contact) confidenceContact += stateFsrRight.confidence;
  else confidenceNoContact += stateFsrRight.confidence;
  if(stateLoad.contact) confidenceContact += stateLoad.confidence;
  else confidenceNoContact += stateLoad.confidence;

  confidenceContactBuffer.add(confidenceContact);
  confidenceNoContactBuffer.add(confidenceNoContact);
  if(confidenceContactBuffer.getSum() >= BUFFER_SIZE * p.contactThreshold)
    contact = true;
  else if(confidenceNoContactBuffer.getSum() >= BUFFER_SIZE * p.noContactThreshold)
    contact = false;

  groundContactState.contact = contact || theMotionRequest.motion == MotionRequest::specialAction || theMotionInfo.motion == MotionRequest::specialAction;
  PLOT("module:GroundContactDetector:contactThreshold", BUFFER_SIZE * p.contactThreshold);
  PLOT("module:GroundContactDetector:noContactThreshold", BUFFER_SIZE * p.noContactThreshold);
  PLOT("module:GroundContactDetector:confidenceContact", confidenceContactBuffer.getSum());
  PLOT("module:GroundContactDetector:confidenceNoContact", confidenceNoContactBuffer.getSum());

  if((contact && !lastContact) || (contact && contactStartTime == 0))
    contactStartTime = theFrameInfo.time;
  //groundContactState.contactSafe = contact && theFrameInfo.getTimeSince(contactStartTime) >= p.safeContactTime;
  if(!contact && lastContact)
  {
#ifndef TARGET_SIM
    if(contactStartTime != 0 && theMotionInfo.motion == MotionRequest::walk)
      SoundPlayer::play("high.wav");
#endif
  }

  lastContact = contact;
}
コード例 #29
0
void KickEngineData::ModifyData(const KickRequest& br, JointRequest& kickEngineOutput, std::vector<KickEngineParameters>& params)
{
  auto& p = params.back();
  MODIFY("module:KickEngine:newKickMotion", p);
  strcpy(p.name, "newKick");

  MODIFY("module:KickEngine:px", gyroP.x());
  MODIFY("module:KickEngine:dx", gyroD.x());
  MODIFY("module:KickEngine:ix", gyroI.x());
  MODIFY("module:KickEngine:py", gyroP.y());
  MODIFY("module:KickEngine:dy", gyroD.y());
  MODIFY("module:KickEngine:iy", gyroI.y());

  MODIFY("module:KickEngine:formMode", formMode);
  MODIFY("module:KickEngine:lFootTraOff", limbOff[Phase::leftFootTra]);
  MODIFY("module:KickEngine:rFootTraOff", limbOff[Phase::rightFootTra]);
  MODIFY("module:KickEngine:lFootRotOff", limbOff[Phase::leftFootRot]);
  MODIFY("module:KickEngine:rFootRotOff", limbOff[Phase::rightFootRot]);
  MODIFY("module:KickEngine:lHandTraOff", limbOff[Phase::leftArmTra]);
  MODIFY("module:KickEngine:rHandTraOff", limbOff[Phase::rightArmTra]);
  MODIFY("module:KickEngine:lHandRotOff", limbOff[Phase::leftHandRot]);
  MODIFY("module:KickEngine:rHandRotOff", limbOff[Phase::rightHandRot]);

  //Plot com stabilizing
  PLOT("module:KickEngine:comy", robotModel.centerOfMass.y());
  PLOT("module:KickEngine:diffy", actualDiff.y());
  PLOT("module:KickEngine:refy", ref.y());

  PLOT("module:KickEngine:comx", robotModel.centerOfMass.x());
  PLOT("module:KickEngine:diffx", actualDiff.x());
  PLOT("module:KickEngine:refx", ref.x());

  PLOT("module:KickEngine:lastdiffy", toDegrees(lastBody.y()));
  PLOT("module:KickEngine:bodyErrory", toDegrees(bodyError.y()));

  for(int i = 0; i < Phase::numOfLimbs; i++)
  {
    const int stiffness = limbOff[i] ? 0 : 100;

    switch(static_cast<Phase::Limb>(i))
    {
      case Phase::leftFootTra:
        kickEngineOutput.stiffnessData.stiffnesses[Joints::lHipRoll] = stiffness;
        kickEngineOutput.stiffnessData.stiffnesses[Joints::lHipPitch] = stiffness;
        kickEngineOutput.stiffnessData.stiffnesses[Joints::lKneePitch] = stiffness;
        kickEngineOutput.stiffnessData.stiffnesses[Joints::lAnklePitch] = stiffness;
        kickEngineOutput.stiffnessData.stiffnesses[Joints::lAnkleRoll] = stiffness;
        break;
      case Phase::rightFootTra:
        kickEngineOutput.stiffnessData.stiffnesses[Joints::rHipRoll] = stiffness;
        kickEngineOutput.stiffnessData.stiffnesses[Joints::rHipPitch] = stiffness;
        kickEngineOutput.stiffnessData.stiffnesses[Joints::rKneePitch] = stiffness;
        kickEngineOutput.stiffnessData.stiffnesses[Joints::rAnklePitch] = stiffness;
        kickEngineOutput.stiffnessData.stiffnesses[Joints::rAnkleRoll] = stiffness;
        break;
      case Phase::leftFootRot:
        kickEngineOutput.stiffnessData.stiffnesses[Joints::lAnklePitch] = stiffness;
        kickEngineOutput.stiffnessData.stiffnesses[Joints::lAnkleRoll] = stiffness;
        kickEngineOutput.stiffnessData.stiffnesses[Joints::rHipYawPitch] = stiffness;
        kickEngineOutput.stiffnessData.stiffnesses[Joints::lHipYawPitch] = stiffness;
        break;
      case Phase::rightFootRot:
        kickEngineOutput.stiffnessData.stiffnesses[Joints::rAnklePitch] = stiffness;
        kickEngineOutput.stiffnessData.stiffnesses[Joints::rAnkleRoll] = stiffness;
        kickEngineOutput.stiffnessData.stiffnesses[Joints::rHipYawPitch] = stiffness;
        kickEngineOutput.stiffnessData.stiffnesses[Joints::lHipYawPitch] = stiffness;
        break;
      case Phase::leftArmTra:
        kickEngineOutput.stiffnessData.stiffnesses[Joints::lShoulderPitch] = stiffness;
        kickEngineOutput.stiffnessData.stiffnesses[Joints::lShoulderRoll] = stiffness;
        break;
      case Phase::rightArmTra:
        kickEngineOutput.stiffnessData.stiffnesses[Joints::rShoulderPitch] = stiffness;
        kickEngineOutput.stiffnessData.stiffnesses[Joints::rShoulderRoll] = stiffness;
        break;
      case Phase::leftHandRot:
        kickEngineOutput.stiffnessData.stiffnesses[Joints::lElbowRoll] = stiffness;
        kickEngineOutput.stiffnessData.stiffnesses[Joints::lElbowYaw] = stiffness;
        kickEngineOutput.stiffnessData.stiffnesses[Joints::lWristYaw] = stiffness;
        kickEngineOutput.stiffnessData.stiffnesses[Joints::lHand] = stiffness;
        break;
      case Phase::rightHandRot:
        kickEngineOutput.stiffnessData.stiffnesses[Joints::rElbowRoll] = stiffness;
        kickEngineOutput.stiffnessData.stiffnesses[Joints::rElbowYaw] = stiffness;
        kickEngineOutput.stiffnessData.stiffnesses[Joints::rWristYaw] = stiffness;
        kickEngineOutput.stiffnessData.stiffnesses[Joints::rHand] = stiffness;
        break;
    }
  }
}
コード例 #30
0
void GroundContactDetector::update(GroundContactState& groundContactState)
{
  DECLARE_PLOT("module:GroundContactDetector:angleNoiseX");
  DECLARE_PLOT("module:GroundContactDetector:angleNoiseY");
  DECLARE_PLOT("module:GroundContactDetector:accNoiseX");
  DECLARE_PLOT("module:GroundContactDetector:accNoiseY");
  DECLARE_PLOT("module:GroundContactDetector:accNoiseZ");
  DECLARE_PLOT("module:GroundContactDetector:gyroNoiseX");
  DECLARE_PLOT("module:GroundContactDetector:gyroNoiseY");

  MODIFY("module:GroundContactDetector:contact", contact);

  if(theMotionInfo.motion == MotionRequest::getUp) //hack to avoid long pause after get up
  {
    contact = true;
    useAngle = false;
    groundContactState.contact = contact;
    contactStartTime = theFrameInfo.time;
  }

  bool ignoreSensors = (theMotionInfo.motion != MotionRequest::walk && theMotionInfo.motion != MotionRequest::stand &&
                        theMotionInfo.motion != MotionRequest::specialAction && theMotionInfo.motion != MotionRequest::getUp) ||
                       (theMotionRequest.motion != MotionRequest::walk && theMotionRequest.motion != MotionRequest::stand &&
                        theMotionRequest.motion != MotionRequest::specialAction && theMotionRequest.motion != MotionRequest::getUp) ||
                       (theMotionInfo.motion == MotionRequest::walk && theMotionInfo.walkRequest.kickType != WalkRequest::none) ||
                       (theMotionRequest.motion == MotionRequest::walk && theMotionRequest.walkRequest.kickType != WalkRequest::none) ||
                       (theMotionInfo.motion == MotionRequest::specialAction && theMotionInfo.specialActionRequest.specialAction != SpecialActionRequest::standHigh) ||
                       (theMotionRequest.motion == MotionRequest::specialAction && theMotionRequest.specialActionRequest.specialAction != SpecialActionRequest::standHigh);
  if(!ignoreSensors)
  {
    if(contact)
    {
      calibratedAccZValues.push_front(theInertialData.acc.z());

      Vector3f angleDiff = (theTorsoMatrix.rotation * expectedRotationInv).getPackedAngleAxis();
      angleNoises.push_front(Vector2f(sqr(angleDiff.x()), sqr(angleDiff.y())));
      Vector2f angleNoise = angleNoises.average();
      PLOT("module:GroundContactDetector:angleNoiseX", angleNoise.x());
      PLOT("module:GroundContactDetector:angleNoiseY", angleNoise.y());

      if(!useAngle && angleNoises.full() && angleNoise.x() < contactAngleActivationNoise && angleNoise.y() < contactAngleActivationNoise)
        useAngle = true;

      if((useAngle && (angleNoise.x() > contactMaxAngleNoise || angleNoise.y() > contactMaxAngleNoise)) ||
         (calibratedAccZValues.full() && calibratedAccZValues.average() > contactMaxAccZ))
      {
        /*
        if((useAngle && (angleNoise.x > p.contactMaxAngleNoise || angleNoise.y > p.contactMaxAngleNoise)))
          OUTPUT_ERROR("lost ground contact via angle");
        if((calibratedAccZValues.isFull() && calibratedAccZValues.getAverage() > p.contactMaxAccZ))
          OUTPUT_ERROR("lost ground contact via acc");
        */

        contact = false;
        accNoises.clear();
        gyroNoises.clear();
        accValues.clear();
        gyroValues.clear();
        angleNoises.clear();
        if(SystemCall::getMode() == SystemCall::physicalRobot && contactStartTime != 0)
          SystemCall::playSound("high.wav");
      }
    }
    else
    {
      const Vector3f accAverage = accValues.average();
      const Vector2f gyroAverage = gyroValues.average();
      const Vector2f gyro = theInertialData.gyro.head<2>().cast<float>();
      const Vector3f acc = theInertialData.acc.cast<float>();
      accValues.push_front(acc);
      gyroValues.push_front(gyro);
      if(accValues.full())
      {
        accNoises.push_front((acc - accAverage).array().square());
        gyroNoises.push_front((gyro - gyroAverage).array().square());
      }
      Vector3f accNoise = accNoises.average();
      Vector2f gyroNoise = gyroNoises.average();
      PLOT("module:GroundContactDetector:accNoiseX", accNoise.x());
      PLOT("module:GroundContactDetector:accNoiseY", accNoise.y());
      PLOT("module:GroundContactDetector:accNoiseZ", accNoise.z());
      PLOT("module:GroundContactDetector:gyroNoiseX", gyroNoise.x());
      PLOT("module:GroundContactDetector:gyroNoiseY", gyroNoise.y());

      if(accNoises.full() &&
         std::abs(accAverage.z() - Constants::g_1000) < 5.f && std::abs(accAverage.x()) < 5.f && std::abs(accAverage.y()) < 5.f &&
         accNoise.x() < noContactMinAccNoise && accNoise.y() < noContactMinAccNoise && accNoise.z() < noContactMinAccNoise &&
         gyroNoise.x() < noContactMinGyroNoise && gyroNoise.y() < noContactMinGyroNoise)
      {
        contact = true;
        useAngle = false;
        contactStartTime = theFrameInfo.time;
        angleNoises.clear();
        calibratedAccZValues.clear();
      }
    }
  }

  groundContactState.contact = contact;

  expectedRotationInv = theRobotModel.limbs[Limbs::footLeft].translation.z() > theRobotModel.limbs[Limbs::footRight].translation.z() ? theRobotModel.limbs[Limbs::footLeft].rotation : theRobotModel.limbs[Limbs::footRight].rotation;
}