Exemplo n.º 1
0
void KickEngineData::simpleCalcArmJoints(const Joints::Joint& joint, JointRequest& jointRequest, const RobotDimensions& theRobotDimensions,
                                         const Vector3f& armPos, const Vector3f& handRotAng)
{
  float sign = joint == Joints::lShoulderPitch ? 1.f : -1.f;

  const Vector3f target = armPos - Vector3f(theRobotDimensions.armOffset.x(), theRobotDimensions.armOffset.y() * sign, theRobotDimensions.armOffset.z());

  jointRequest.angles[joint + 0] = std::atan2(target.z(), target.x());
  jointRequest.angles[joint + 1] = std::atan2(target.y() * sign, std::sqrt(sqr(target.x()) + sqr(target.z())));

  float length2ElbowJoint = Vector3f(theRobotDimensions.upperArmLength, theRobotDimensions.yOffsetElbowToShoulder, 0.f).norm();
  float angle = std::asin(theRobotDimensions.upperArmLength / length2ElbowJoint);

  Pose3f elbow;
  elbow.rotateY(-jointRequest.angles[joint + 0])
  .rotateZ(jointRequest.angles[joint + 1])
  .translate(length2ElbowJoint, 0.f, 0.f)
  .rotateZ(-angle)
  .translate(theRobotDimensions.yOffsetElbowToShoulder, 0.f, 0.f);

  jointRequest.angles[joint + 0] = std::atan2(elbow.translation.z(), elbow.translation.x());
  jointRequest.angles[joint + 1] = std::atan2(elbow.translation.y(), std::sqrt(sqr(elbow.translation.x()) + sqr(elbow.translation.z())));
  jointRequest.angles[joint + 0] = (jointRequest.angles[joint + 0] < pi) ? jointRequest.angles[joint + 0] : 0_deg;  //clip special

  jointRequest.angles[joint + 0] *= -1.f;
  jointRequest.angles[joint + 1] *= sign;
  jointRequest.angles[joint + 2] = handRotAng.x();
  jointRequest.angles[joint + 3] = handRotAng.y();
  jointRequest.angles[joint + 4] = handRotAng.z();
  jointRequest.angles[joint + 5] = 0.f;
}
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());
}
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();
}
Exemplo n.º 4
0
Pose3f KickViewMath::calculateFootPos(const JointAngles& jointAngles, const Joints::Joint& joint, const RobotDimensions& robotDimensions)
{
  Pose3f footPos;
  float sign = joint == Joints::lHipYawPitch ? -1.f : 1.f;
  footPos.translate(0, -sign * (robotDimensions.yHipOffset), 0)
  .rotateX(-pi_4 * sign)
  .rotateZ(jointAngles.angles[joint]*sign)
  .rotateX(((jointAngles.angles[joint + 1] + pi_4)*sign))
  .rotateY(jointAngles.angles[joint + 2])
  .translate(0, 0, -robotDimensions.upperLegLength)
  .rotateY(jointAngles.angles[joint + 3])
  .translate(0, 0, -robotDimensions.lowerLegLength);

  footPos.translation += Vector3f(0, 0, -robotDimensions.footHeight); //because inverse kinematics subtracts this

  return footPos;
}
LIPStateEstimator::EstimatedState LIPStateEstimator::convertToOtherFoot(const EstimatedState& state) const
{
  EstimatedState otherState(state);
  otherState.supportFoot = state.supportFoot == SupportFoot::left ? SupportFoot::right : SupportFoot::left;
  otherState.origin.y() *= -1.f;

  const Pose3f& soleToTorso = state.supportFoot == SupportFoot::left ? theRobotModel.soleLeft : theRobotModel.soleRight;
  const Pose3f& otherSoleToTorso = state.supportFoot == SupportFoot::left ? theRobotModel.soleRight : theRobotModel.soleLeft;
  const Pose3f originToTorso = soleToTorso + Vector3f(state.origin.x(), state.origin.y(), 0.f);
  const Pose3f otherOriginToTorso = otherSoleToTorso + Vector3f(otherState.origin.x(), otherState.origin.y(), 0.f);

  //const Quaternionf& torsoToWorld = theInertiaData.orientation;
  //Pose3f oritinToOtherOrigin = Pose3f(torsoToWorld) * otherOriginToTorso.rotation * otherOriginToTorso.inverse() * originToTorso * originToTorso.rotation.inverse() *= torsoToWorld.inverse();

  const Pose3f oritinToOtherOrigin = otherOriginToTorso.inverse() * originToTorso;

  otherState.com.position = (oritinToOtherOrigin * Vector3f(state.com.position.x(), state.com.position.y(), 0.f)).head<2>();
  otherState.com.velocity = (oritinToOtherOrigin.rotation * Vector3f(state.com.velocity.x(), state.com.velocity.y(), 0.f)).head<2>();
  otherState.zmp = (oritinToOtherOrigin * Vector3f(state.zmp.x(), state.zmp.y(), 0.f)).head<2>();
  return otherState;
}
Exemplo n.º 6
0
Pose3f KickViewMath::calculateHandPos(const JointAngles& jointAngles, const Joints::Joint& joint, const RobotDimensions& robotDimensions)
{
  Pose3f handPos;
  float sign = joint == Joints::lShoulderPitch ? 1.f : -1.f;

  handPos.translate(robotDimensions.armOffset.x(), sign *  robotDimensions.armOffset.y(), robotDimensions.armOffset.z());

  handPos.rotateY(-jointAngles.angles[joint]);
  handPos.rotateZ(sign * jointAngles.angles[joint + 1]);
  handPos.translate(robotDimensions.upperArmLength, 0, 0);

  handPos.rotateX(jointAngles.angles[joint + 2] * sign);
  handPos.rotateZ(sign * jointAngles.angles[joint + 3]);

  return handPos;
}
void InertialDataFilter::update(InertialData& inertialData)
{
  DECLARE_PLOT("module:InertialDataFilter:expectedAccX");
  DECLARE_PLOT("module:InertialDataFilter:accX");
  DECLARE_PLOT("module:InertialDataFilter:expectedAccY");
  DECLARE_PLOT("module:InertialDataFilter:accY");
  DECLARE_PLOT("module:InertialDataFilter:expectedAccZ");
  DECLARE_PLOT("module:InertialDataFilter:accZ");

  // check whether the filter shall be reset
  if(!lastTime || theFrameInfo.time <= lastTime)
  {
    if(theFrameInfo.time == lastTime)
      return; // weird log file replaying?
    reset();
  }

  if(theMotionInfo.motion == MotionRequest::specialAction && theMotionInfo.specialActionRequest.specialAction == SpecialActionRequest::playDead)
  {
    reset();
  }

  // get foot positions
  Pose3f leftFoot = theRobotModel.limbs[Limbs::footLeft];
  Pose3f rightFoot = theRobotModel.limbs[Limbs::footRight];
  leftFoot.translate(0.f, 0.f, -theRobotDimensions.footHeight);
  rightFoot.translate(0.f, 0.f, -theRobotDimensions.footHeight);
  const Pose3f leftFootInvert(leftFoot.inverse());
  const Pose3f rightFootInvert(rightFoot.inverse());

  // calculate rotation and position offset using the robot model (joint data)
  const Pose3f leftOffset(lastLeftFoot.translation.z() != 0.f ? Pose3f(lastLeftFoot).conc(leftFootInvert) : Pose3f());
  const Pose3f rightOffset(lastRightFoot.translation.z() != 0.f ? Pose3f(lastRightFoot).conc(rightFootInvert) : Pose3f());

  // detect the foot that is on ground
  bool useLeft = true;
  if(theMotionInfo.motion == MotionRequest::walk && theWalkingEngineOutput.speed.translation.x() != 0)
  {
    useLeft = theWalkingEngineOutput.speed.translation.x() > 0 ?
              (leftOffset.translation.x() > rightOffset.translation.x()) :
              (leftOffset.translation.x() < rightOffset.translation.x());
  }
  else
  {
    Pose3f left(mean.rotation);
    Pose3f right(mean.rotation);
    left.conc(leftFoot);
    right.conc(rightFoot);
    useLeft = left.translation.z() < right.translation.z();
  }

  // update the filter
  const float timeScale = theFrameInfo.cycleTime;
  predict(RotationMatrix::fromEulerAngles(theInertialSensorData.gyro.x() * timeScale,
                                          theInertialSensorData.gyro.y() * timeScale, 0));

  // insert calculated rotation
  safeRawAngle = theInertialSensorData.angle.head<2>().cast<float>();
  bool useFeet = true;
  MODIFY("module:InertialDataFilter:useFeet", useFeet);
  if(useFeet &&
     (theMotionInfo.motion == MotionRequest::walk || theMotionInfo.motion == MotionRequest::stand ||
      (theMotionInfo.motion == MotionRequest::specialAction && theMotionInfo.specialActionRequest.specialAction == SpecialActionRequest::standHigh)) &&
     std::abs(safeRawAngle.x()) < calculatedAccLimit.x() && std::abs(safeRawAngle.y()) < calculatedAccLimit.y())
  {
    const RotationMatrix& usedRotation(useLeft ? leftFootInvert.rotation : rightFootInvert.rotation);
    Vector3f accGravOnly(usedRotation.col(0).z(), usedRotation.col(1).z(), usedRotation.col(2).z());
    accGravOnly *= Constants::g_1000;
    readingUpdate(accGravOnly);
  }
  else // insert acceleration sensor values
    readingUpdate(theInertialSensorData.acc);

  // fill the representation
  inertialData.angle = Vector2a(std::atan2(mean.rotation.col(1).z(), mean.rotation.col(2).z()), std::atan2(-mean.rotation.col(0).z(), mean.rotation.col(2).z()));

  inertialData.acc = theInertialSensorData.acc;
  inertialData.gyro = theInertialSensorData.gyro;

  inertialData.orientation = Rotation::removeZRotation(Quaternionf(mean.rotation));

  // this  keeps the rotation matrix orthogonal
  mean.rotation.normalize();

  // store some data for the next iteration
  lastLeftFoot = leftFoot;
  lastRightFoot = rightFoot;
  lastTime = theFrameInfo.time;

  // plots
  Vector3f angleAxisVec = Rotation::AngleAxis::pack(AngleAxisf(inertialData.orientation));
  PLOT("module:InertialDataFilter:angleX", toDegrees(angleAxisVec.x()));
  PLOT("module:InertialDataFilter:angleY", toDegrees(angleAxisVec.y()));
  PLOT("module:InertialDataFilter:angleZ", toDegrees(angleAxisVec.z()));
  PLOT("module:InertialDataFilter:unfilteredAngleX", theInertialSensorData.angle.x().toDegrees());
  PLOT("module:InertialDataFilter:unfilteredAngleY", theInertialSensorData.angle.y().toDegrees());

  angleAxisVec = Rotation::AngleAxis::pack(AngleAxisf(mean.rotation));
  PLOT("module:InertialDataFilter:interlanAngleX", toDegrees(angleAxisVec.x()));
  PLOT("module:InertialDataFilter:interlanAngleY", toDegrees(angleAxisVec.y()));
  PLOT("module:InertialDataFilter:interlanAngleZ", toDegrees(angleAxisVec.z()));
}