コード例 #1
0
  public VirtualChainDataPoint(
      ReferenceFrame baseFrame,
      ArrayList<ReferenceFrame> referenceFrames,
      FramePoint2d centerOfMassProjection)
        //   public VirtualChainDataPoint(ReferenceFrame baseFrame, ArrayList<ReferenceFrame>
        // referenceFrames, FramePoint centerOfMassProjection)
      {
    centerOfMassProjection.checkReferenceFrameMatch(baseFrame);

    if (!centerOfMassProjection.getReferenceFrame().isZupFrame()) {
      throw new RuntimeException("centerOfMassProjection is Not a ZUp reference frame!");
    }

    this.centerOfMassProjection = new FramePoint2d(centerOfMassProjection);
    //      centerOfMassProjection.changeFrame(baseFrame);

    rotationMatrices = new ArrayList<Matrix3d>();

    for (int i = 0; i < referenceFrames.size(); i++) {
      ReferenceFrame virtualChainFrame = referenceFrames.get(i);

      RigidBodyTransform transform3D = virtualChainFrame.getTransformToDesiredFrame(baseFrame);

      Matrix3d rotationMatrix = new Matrix3d();
      transform3D.get(rotationMatrix);

      rotationMatrices.add(rotationMatrix);
    }
  }
 public void updateRobotLocationBasedOnMultisensePose(RigidBodyTransform headPose) {
   RigidBodyTransform pelvisPose = new RigidBodyTransform();
   RigidBodyTransform transformFromHeadToPelvis =
       pelvisFrame.getTransformToDesiredFrame(headFrame);
   pelvisPose.multiply(headPose, transformFromHeadToPelvis);
   atomicPelvisPose.set(pelvisPose);
 }
  private Matrix3d computeDesiredFootRotation(
      double angleToDestination, RobotSide swingLegSide, ReferenceFrame supportFootFrame) {
    RigidBodyTransform supportFootToWorldTransform =
        supportFootFrame.getTransformToDesiredFrame(ReferenceFrame.getWorldFrame());
    Matrix3d supportFootToWorldRotation = new Matrix3d();
    supportFootToWorldTransform.get(supportFootToWorldRotation);

    double maxTurnInAngle = 0.25;
    double maxTurnOutAngle = 0.4;

    double amountToYaw;
    switch (blindWalkingDirection.getEnumValue()) {
      case BACKWARD:
        {
          amountToYaw = AngleTools.computeAngleDifferenceMinusPiToPi(angleToDestination, Math.PI);
          break;
        }
      case LEFT:
        {
          amountToYaw =
              AngleTools.computeAngleDifferenceMinusPiToPi(angleToDestination, Math.PI / 2.0);
          break;
        }
      case RIGHT:
        {
          amountToYaw =
              AngleTools.computeAngleDifferenceMinusPiToPi(angleToDestination, -Math.PI / 2.0);
          break;
        }
      case FORWARD:
        {
          amountToYaw = angleToDestination;
          break;
        }
      default:
        {
          throw new RuntimeException("Shouldn't get here!");
        }
    }

    if (swingLegSide == RobotSide.LEFT) {
      amountToYaw = MathTools.clipToMinMax(amountToYaw, -maxTurnInAngle, maxTurnOutAngle);

    } else {
      amountToYaw = MathTools.clipToMinMax(amountToYaw, -maxTurnOutAngle, maxTurnInAngle);
    }

    Matrix3d yawRotation = new Matrix3d();
    yawRotation.rotZ(amountToYaw);

    Matrix3d ret = new Matrix3d();
    ret.mul(yawRotation, supportFootToWorldRotation);

    return ret;
  }
  @ContinuousIntegrationTest(estimatedDuration = 0.0)
  @Test(timeout = 30000)
  public void testCommonUsageExample() {
    String namePrefix = "point";
    String nameSuffix = "toTest";
    YoVariableRegistry registry = new YoVariableRegistry("myRegistry");

    ReferenceFrame worldFrame = ReferenceFrame.getWorldFrame();
    PoseReferenceFrame poseFrame = new PoseReferenceFrame("poseFrame", new FramePose(worldFrame));

    FramePoint poseFramePosition = new FramePoint(worldFrame, new Point3d(0.5, 7.7, 9.2));
    poseFrame.setPositionAndUpdate(poseFramePosition);

    FrameOrientation poseOrientation =
        new FrameOrientation(worldFrame, new AxisAngle4d(1.2, 3.9, 4.7, 2.2));
    poseFrame.setOrientationAndUpdate(poseOrientation);

    YoFrameSO3TrajectoryPoint yoFrameSO3TrajectoryPoint =
        new YoFrameSO3TrajectoryPoint(namePrefix, nameSuffix, registry, worldFrame, poseFrame);
    SimpleSO3TrajectoryPoint simpleTrajectoryPoint = new SimpleSO3TrajectoryPoint();

    double time = 3.4;
    TransformableQuat4d orientation = new TransformableQuat4d(new Quat4d(0.1, 0.22, 0.34, 0.56));
    orientation.normalize();

    Vector3d angularVelocity = new Vector3d(1.7, 8.4, 2.2);

    simpleTrajectoryPoint.set(time, orientation, angularVelocity);
    yoFrameSO3TrajectoryPoint.setIncludingFrame(worldFrame, simpleTrajectoryPoint);

    yoFrameSO3TrajectoryPoint.changeFrame(poseFrame);

    // Do some checks:
    RigidBodyTransform transformToPoseFrame = worldFrame.getTransformToDesiredFrame(poseFrame);
    orientation.applyTransform(transformToPoseFrame);
    transformToPoseFrame.transform(angularVelocity);

    namePrefix = "point";
    nameSuffix = "toVerify";
    YoFrameSO3TrajectoryPoint expectedYoFrameSO3TrajectoryPoint =
        new YoFrameSO3TrajectoryPoint(namePrefix, nameSuffix, registry, poseFrame);

    expectedYoFrameSO3TrajectoryPoint.setTime(time);
    expectedYoFrameSO3TrajectoryPoint.setOrientation(orientation);
    expectedYoFrameSO3TrajectoryPoint.setAngularVelocity(angularVelocity);

    assertEquals(3.4, yoFrameSO3TrajectoryPoint.getTime(), 1e-7);
    assertEquals(3.4, expectedYoFrameSO3TrajectoryPoint.getTime(), 1e-7);

    assertTrue(expectedYoFrameSO3TrajectoryPoint.epsilonEquals(yoFrameSO3TrajectoryPoint, 1e-10));
  }
  private void computeOrientationStateOutputBlock() {
    estimationFrame.getTransformToDesiredFrame(tempTransform, ReferenceFrame.getWorldFrame());
    tempTransform.getRotation(rotationFromEstimationToWorld);

    tempCenterOfMassPosition.setIncludingFrame(centerOfMassPositionPort.getData());
    tempCenterOfMassPosition.changeFrame(estimationFrame);

    ReferenceFrame referenceFrame =
        referenceFrameMap.getFrameByName(
            pointPositionMeasurementInputPort.getData().getBodyFixedReferenceFrameName());
    tempFramePoint.setIncludingFrame(
        referenceFrame,
        pointPositionMeasurementInputPort.getData().getMeasurementPointInBodyFrame());

    tempFramePoint.changeFrame(estimationFrame);
    tempFramePoint.sub(tempCenterOfMassPosition);
    tempFramePoint.scale(-1.0);

    MatrixTools.toTildeForm(tempMatrix, tempFramePoint.getPoint());
    tempMatrix.mul(rotationFromEstimationToWorld, tempMatrix);
    MatrixTools.setDenseMatrixFromMatrix3d(0, 0, tempMatrix, getOutputMatrixBlock(orientationPort));
  }