public DoublePendulum() {
    super("DoublePendulum");

    j1 = new PinJoint("j1", new Vector3d(0, 0, 2), this, Axis.X);

    Link l1 = new Link("l1");
    l1.setComOffset(0, 0, 0.5);
    l1.setMassAndRadiiOfGyration(1.0, 0.05, 0.05, 0.3);
    l1.addEllipsoidFromMassProperties(YoAppearance.Pink());
    j1.setLink(l1);

    j2 = new PinJoint("j2", new Vector3d(0.0, 0.0, 1.0), this, Axis.X);
    Link l2 = new Link("l2");
    l2.setComOffset(0, 0, 0.5);
    l2.setMassAndRadiiOfGyration(1.0, 0.05, 0.05, 0.3);
    l2.addEllipsoidFromMassProperties(YoAppearance.Purple());

    j2.setLink(l2);

    j1.addJoint(j2);
    addRootJoint(j1);
  }
  public FallingSphereRobot(String name, boolean useImpulseGroundModel) {
    super(name);

    this.setGravity(0.0, 0.0, -G);

    // Base:

    floatingJoint = new FloatingJoint("base", new Vector3d(0.0, 0.0, 0.0), this);

    Link link1 = ball();
    floatingJoint.setLink(link1);
    this.addRootJoint(floatingJoint);

    YoGraphicsListRegistry yoGraphicsListRegistry = new YoGraphicsListRegistry();

    // Ground contact points:
    int N = 20; // 8;

    for (int i = 0; i < N; i++) {
      double latitude = -Math.PI / 2.0 + (i * Math.PI) / N;

      int nForThisLatitude = (int) ((Math.cos(latitude) * N) + 0.5);

      for (int j = 0; j < nForThisLatitude; j++) {
        double longitude = (j * 2.0 * Math.PI) / nForThisLatitude;

        double z = R * Math.sin(latitude);
        double x = R * Math.cos(latitude) * Math.cos(longitude);
        double y = R * Math.cos(latitude) * Math.sin(longitude);

        // System.out.println("x,y,z: " + x + ", " + y + ", " + z);
        String gcName = "gc" + i + "_" + j;
        GroundContactPoint gc = new GroundContactPoint(gcName, new Vector3d(x, y, z), this);
        floatingJoint.addGroundContactPoint(gc);

        YoGraphicPosition dynamicGraphicPosition =
            new YoGraphicPosition(
                gcName + "Position", gc.getYoPosition(), 0.01, YoAppearance.Red());
        yoGraphicsListRegistry.registerYoGraphic("FallingSphereGCPoints", dynamicGraphicPosition);

        if (useImpulseGroundModel) {
          YoGraphicVector dynamicGraphicVector =
              new YoGraphicVector(
                  gcName + "Force",
                  gc.getYoPosition(),
                  gc.getYoImpulse(),
                  10.0,
                  YoAppearance.Pink());
          yoGraphicsListRegistry.registerYoGraphic("FallingSphereForces", dynamicGraphicVector);
        } else {
          YoGraphicVector dynamicGraphicVector =
              new YoGraphicVector(
                  gcName + "Force", gc.getYoPosition(), gc.getYoForce(), 1.0 / 50.0);
          yoGraphicsListRegistry.registerYoGraphic("FallingSphereForces", dynamicGraphicVector);
        }
      }
    }

    GroundContactModel groundContactModel;
    if (useImpulseGroundModel) {
      groundContactModel = new CollisionGroundContactModel(this, EPSILON, MU, registry);
    } else {
      double kXY = 1000.0; // 1422.0;
      double bXY = 100.0; // 150.6;
      double kZ = 20.0; // 50.0;
      double bZ = 50.0; // 1000.0;

      groundContactModel = new LinearGroundContactModel(this, kXY, bXY, kZ, bZ, registry);
    }

    double amplitude = 0.1;
    double frequency = 0.3;
    double offset = 0.5;
    GroundProfile3D groundProfile = new RollingGroundProfile(amplitude, frequency, offset);
    groundContactModel.setGroundProfile3D(groundProfile);
    this.setGroundContactModel(groundContactModel);

    initRobot();

    this.getRobotsYoVariableRegistry().addChild(registry);
    this.addDynamicGraphicObjectsListRegistry(yoGraphicsListRegistry);
  }
  public CirclePoseTrajectoryGenerator(
      String namePrefix,
      ReferenceFrame trajectoryFrame,
      DoubleProvider trajectoryTimeProvider,
      YoVariableRegistry parentRegistry,
      YoGraphicsListRegistry yoGraphicsListRegistry) {
    this.registry = new YoVariableRegistry(namePrefix + getClass().getSimpleName());
    this.desiredTrajectoryTime = new DoubleYoVariable(namePrefix + "TrajectoryTime", registry);
    this.currentTime = new DoubleYoVariable(namePrefix + "Time", registry);

    //      this.anglePolynomial = new YoPolynomial(namePrefix + "ParameterPolynomial", 2,
    // registry);
    this.anglePolynomial = new YoPolynomial(namePrefix + "ParameterPolynomial", 4, registry);
    //      this.anglePolynomial = new YoPolynomial(namePrefix + "ParameterPolynomial", 6,
    // registry);

    this.desiredTrajectoryTimeProvider = trajectoryTimeProvider;

    this.trajectoryFrame = trajectoryFrame;

    initialRadius = new DoubleYoVariable(namePrefix + "Radius", registry);
    initialZ = new DoubleYoVariable(namePrefix + "ZPosition", registry);
    initialAngle = new DoubleYoVariable(namePrefix + "InitialAngle", registry);
    finalAngle = new DoubleYoVariable(namePrefix + "FinalAngle", registry);

    isCurrentAngleBeingAdjusted =
        new BooleanYoVariable(namePrefix + "IsCurrentAngleBeingAdjusted", registry);
    maximumAngleTrackingErrorTolerated =
        new DoubleYoVariable(namePrefix + "MaxAngleTrackingErrorTolerated", registry);
    maximumAngleTrackingErrorTolerated.set(Math.toRadians(30.0));
    currentControlledFrameRelativeAngle =
        new DoubleYoVariable(namePrefix + "CurrentControlledFrameAngle", registry);
    currentAngleTrackingError =
        new DoubleYoVariable(namePrefix + "CurrentAngleTrackingError", registry);
    currentAdjustedRelativeAngle =
        new DoubleYoVariable(namePrefix + "CurrentAdjustedRelativeAngle", registry);

    desiredRotationAngle = new DoubleYoVariable(namePrefix + "DesiredRotationAngle", registry);
    currentRelativeAngle = new DoubleYoVariable(namePrefix + "CurrentRelativeAngle", registry);

    yoInitialPosition = new YoFramePoint(namePrefix + "InitialPosition", trajectoryFrame, registry);
    yoFinalPosition = new YoFramePoint(namePrefix + "FinalPosition", trajectoryFrame, registry);

    yoCurrentPosition = new YoFramePoint(namePrefix + "CurrentPosition", trajectoryFrame, registry);
    yoCurrentVelocity =
        new YoFrameVector(namePrefix + "CurrentVelocity", trajectoryFrame, registry);
    yoCurrentAcceleration =
        new YoFrameVector(namePrefix + "CurrentAcceleration", trajectoryFrame, registry);

    yoInitialOrientation =
        new YoFrameQuaternion(namePrefix + "InitialOrientation", trajectoryFrame, registry);
    yoFinalOrientation =
        new YoFrameQuaternion(namePrefix + "FinalOrientation", trajectoryFrame, registry);

    yoCurrentOrientation =
        new YoFrameQuaternion(namePrefix + "CurrentOrientation", trajectoryFrame, registry);
    yoCurrentAngularVelocity =
        new YoFrameVector(namePrefix + "CurrentAngularVelocity", trajectoryFrame, registry);
    yoCurrentAngularAcceleration =
        new YoFrameVector(namePrefix + "CurrentAngularAcceleration", trajectoryFrame, registry);

    yoInitialPositionWorld =
        new YoFramePoint(namePrefix + "InitialPositionWorld", worldFrame, registry);
    yoFinalPositionWorld =
        new YoFramePoint(namePrefix + "FinalPositionWorld", worldFrame, registry);
    yoCurrentPositionWorld =
        new YoFramePoint(namePrefix + "CurrentPositionWorld", worldFrame, registry);
    yoCurrentAdjustedPositionWorld =
        new YoFramePoint(namePrefix + "CurrentAdjustedPositionWorld", worldFrame, registry);

    circleOrigin = new YoFramePoint(namePrefix + "CircleOrigin", trajectoryFrame, registry);
    rotationAxis = new YoFrameVector(namePrefix + "RotationAxis", trajectoryFrame, registry);
    rotationAxis.set(0.0, 0.0, 1.0);

    circleFrame =
        new ReferenceFrame("CircleFrame", trajectoryFrame) {
          private static final long serialVersionUID = 9102217353690768074L;

          private final Vector3d localTranslation = new Vector3d();
          private final Vector3d localRotationAxis = new Vector3d();
          private final AxisAngle4d localAxisAngle = new AxisAngle4d();

          @Override
          protected void updateTransformToParent(RigidBodyTransform transformToParent) {
            circleOrigin.get(localTranslation);
            rotationAxis.get(localRotationAxis);
            GeometryTools.getRotationBasedOnNormal(localAxisAngle, localRotationAxis);
            transformToParent.set(localAxisAngle, localTranslation);
          }
        };

    tangentialCircleFrame = new PoseReferenceFrame("TangentialCircleFrame", circleFrame);
    yoTangentialCircleFramePose =
        new YoFramePose(namePrefix + "TangentialCircleFramePose", worldFrame, registry);

    if (this.visualize && yoGraphicsListRegistry != null) {
      final YoGraphicPosition currentPositionViz =
          new YoGraphicPosition(
              namePrefix + "CurrentPosition", yoCurrentPositionWorld, 0.025, YoAppearance.Blue());
      final YoGraphicPosition currentAdjustedPositionViz =
          new YoGraphicPosition(
              namePrefix + "CurrentAdjustedPosition",
              yoCurrentAdjustedPositionWorld,
              0.023,
              YoAppearance.Gold());
      final YoGraphicPosition initialPositionViz =
          new YoGraphicPosition(
              namePrefix + "InitialPosition",
              yoInitialPositionWorld,
              0.02,
              YoAppearance.BlueViolet());
      final YoGraphicPosition finalPositionViz =
          new YoGraphicPosition(
              namePrefix + "FinalPosition", yoFinalPositionWorld, 0.02, YoAppearance.Red());
      final YoGraphicCoordinateSystem tangentialFrameViz =
          new YoGraphicCoordinateSystem(
              namePrefix + "TangentialFrame",
              yoTangentialCircleFramePose,
              0.2,
              YoAppearance.Pink());

      YoGraphicsList yoGraphicsList = new YoGraphicsList(namePrefix + "CircleTraj");
      yoGraphicsList.add(currentPositionViz);
      yoGraphicsList.add(currentAdjustedPositionViz);
      yoGraphicsList.add(initialPositionViz);
      yoGraphicsList.add(finalPositionViz);
      yoGraphicsList.add(tangentialFrameViz);
      yoGraphicsListRegistry.registerYoGraphicsList(yoGraphicsList);

      bagOfBalls =
          new BagOfBalls(
              numberOfBalls, 0.01, yoGraphicsList.getLabel(), registry, yoGraphicsListRegistry);

      showViz = new BooleanYoVariable(namePrefix + "ShowViz", registry);
      showViz.addVariableChangedListener(
          new VariableChangedListener() {
            public void variableChanged(YoVariable<?> v) {
              boolean visible = showViz.getBooleanValue();
              currentPositionViz.setVisible(visible);
              currentAdjustedPositionViz.setVisible(visible);
              initialPositionViz.setVisible(visible);
              finalPositionViz.setVisible(visible);
              tangentialFrameViz.setVisible(visible);
              bagOfBalls.setVisible(visible);
            }
          });
      showViz.notifyVariableChangedListeners();
    } else {
      visualize = false;
      bagOfBalls = null;
      showViz = null;
    }

    parentRegistry.addChild(registry);
  }