public SimpleTableTerrainObject(
      double xStart, double yStart, double xEnd, double yEnd, double zStart, double zEnd) {
    this.TABLE_LENGTH = Math.abs(xStart - xEnd);
    this.TABLE_WIDTH = Math.abs(yStart - yEnd);
    this.TABLE_THICKNESS = Math.abs(zStart - zEnd);

    double xMin = Math.min(xStart, xEnd);
    double xMax = Math.max(xStart, xEnd);

    double yMin = Math.min(yStart, yEnd);
    double yMax = Math.max(yStart, yEnd);

    double zMin = Math.min(zStart, zEnd);
    double zMax = Math.max(zStart, zEnd);

    Point3d minPoint = new Point3d(xMin, yMin, zMin);
    Point3d maxPoint = new Point3d(xMax, yMax, zMax);

    boundingBox = new BoundingBox3d(minPoint, maxPoint);

    linkGraphics = new Graphics3DObject();

    linkGraphics.translate(
        (xStart + xEnd) / 2.0, (yStart + yEnd) / 2.0, zMin + TABLE_THICKNESS / 2);
    linkGraphics.scale(new Vector3d(TABLE_LENGTH, TABLE_WIDTH, TABLE_THICKNESS));
    linkGraphics.addModelFile("models/plasticTableTop.obj");

    if (TABLE_LENGTH < TABLE_WIDTH) linkGraphics.rotate(Math.PI / 2, Axis.Z);
    linkGraphics.scale(new Vector3d(1, 1, boundingBox.getZMax() / TABLE_THICKNESS));
    linkGraphics.addModelFile("models/FoldingTableLegs.obj");
  }
  /** Joints */
  public Step2Robot() {
    super("v2Robot");
    this.setGravity(-9.81);

    // Body (2DOF = Z + Pitch)
    bodyJoint1 = new SliderJoint("bodyZ", new Vector3d(0.0, 0.0, 0.0), this, Axis.Z);
    bodyJoint1.setDynamic(true);
    Link bodyLinkSlider = setNullLink();
    bodyJoint1.setLink(bodyLinkSlider);
    this.addRootJoint(bodyJoint1);
    bodyJoint1.setInitialState(legHeight, 0.0);

    bodyJoint2 = new PinJoint("bodyPitch", new Vector3d(0.0, 0.0, 0.0), this, Axis.Y);
    bodyJoint2.setDynamic(true);
    Link bodyLinkPitch = body();
    bodyJoint2.setLink(bodyLinkPitch);
    bodyJoint1.addJoint(bodyJoint2);

    // Upper Joint
    hipJoint = new PinJoint("hip", new Vector3d(0.0, 0.0, 0.0), this, Axis.Y);
    hipJoint.setDynamic(true);
    hipJoint.setLimitStops(0.0, 1.0, 1e6, 1e3);
    Link upperLink = upperLink();
    hipJoint.setLink(upperLink);
    bodyJoint2.addJoint(hipJoint);

    // Lower Joint
    kneeJoint = new SliderJoint("knee", new Vector3d(0.0, 0.0, -upperLinkLength), this, Axis.Z);
    kneeJoint.setDynamic(true);
    kneeJoint.setLimitStops(0.0, 0.6, 1e9, 1e2);
    Link lowerLink = lowerLink();
    kneeJoint.setLink(lowerLink);
    hipJoint.addJoint(kneeJoint);

    // Visible ground contact point
    GroundContactPoint contactPoint =
        new GroundContactPoint("Foot", new Vector3d(0.0, 0.0, gcOffset), this);
    kneeJoint.addGroundContactPoint(contactPoint);
    Graphics3DObject graphics = kneeJoint.getLink().getLinkGraphics();
    graphics.identity();
    graphics.translate(0.0, 0.0, gcOffset);
    double radius = 0.03;
    graphics.addSphere(radius, YoAppearance.Orange());

    // Ground Model
    GroundContactModel groundModel =
        new LinearGroundContactModel(
            this, 1500, 150, 50000.0, 1e5, this.getRobotsYoVariableRegistry());
    GroundProfile3D profile = new FlatGroundProfile();
    groundModel.setGroundProfile3D(profile);
    this.setGroundContactModel(groundModel);
  }
  public static void addCube(
      double cubeX, double cubeY, double cubeSize, Color color, SimulationConstructionSet scs) {
    final double CUBE_HEIGHT = 0.001;
    // Make cubes a tiny bit smaller so they don't blend together.
    cubeSize *= 0.9;

    Graphics3DObject linkGraphics = new Graphics3DObject();
    linkGraphics.translate(new Vector3d(cubeX, cubeY, CUBE_HEIGHT));
    linkGraphics.addCube(
        cubeSize, cubeSize, cubeSize, new YoAppearanceRGBColor(new Color3f(color), 0.0));

    scs.addStaticLinkGraphics(linkGraphics);
  }
  private void createAndAttachFootLink(
      LinkNames linkName, JointNames jointName, RobotSide robotSide) {
    Link link = new Link(LinkNames.FOOT_LINK.getName());
    Matrix3d inertiaFoot = createInertiaMatrixBox(linkName);
    link.setMomentOfInertia(inertiaFoot);
    link.setMass(RobotParameters.MASSES.get(LinkNames.FOOT_LINK));
    link.setComOffset(comOffsetFoot);

    Graphics3DObject linkGraphics = new Graphics3DObject();
    linkGraphics.translate(footOffsetX, 0.0, 0.0);
    linkGraphics.addCube(footX, footY, -footZ, RobotParameters.APPEARANCE.get(LinkNames.FOOT_LINK));
    link.setLinkGraphics(linkGraphics);
    //      link.addEllipsoidFromMassProperties(YoAppearance.Green());
    link.addCoordinateSystemToCOM(0.3);
    idToSCSLegJointMap.get(allLegJoints.get(robotSide).get(jointName)).setLink(link);
  }
  /** This method returns a brick link instance. */
  private Link base(String name, AppearanceDefinition appearance) {
    Link ret = new Link(name);
    ret.setMass(M1);
    ret.setMomentOfInertia(Ixx1, Iyy1, Izz1);
    ret.setComOffset(0.0, 0.0, 0.0);

    Graphics3DObject linkGraphics = new Graphics3DObject();
    linkGraphics.translate(0.0, 0.0, -B1);

    // linkGraphics.addCube((float)BASE_L, (float)BASE_W, (float)BASE_H, appearance);
    // linkGraphics.addCone((float)BASE_L,(float)BASE_W);
    linkGraphics.addPyramidCube(BASE_L, BASE_W, BASE_H, BASE_H, appearance);

    ret.setLinkGraphics(linkGraphics);

    return ret;
  }
  /** Joints */
  public Step6IDandSCSRobot_pinKnee() {
    super("Robot");

    /** **************** ID ROBOT ********************** */
    elevator = new RigidBody("elevator", elevatorFrame);

    bodyJointID = new SixDoFJoint(JointNames.BODY.getName(), elevator, elevatorFrame);
    createAndAttachBodyRB(LinkNames.BODY_LINK, bodyJointID);

    for (RobotSide robotSide : RobotSide.values) {
      // HIP ID (location, joint, and rigidBody)
      Vector3d hipOffset = new Vector3d(0.0, robotSide.negateIfRightSide(hipOffsetY), 0.0);
      RevoluteJoint hipJointID =
          ScrewTools.addRevoluteJoint(
              JointNames.HIP.getName(),
              bodyJointID.getSuccessor(),
              hipOffset,
              jointAxesPinJoints); // The parent rigid body of the hip joint is:
                                   // bodyJointID.getSuccessor()
      allLegJoints.get(robotSide).put(JointNames.HIP, hipJointID);
      createAndAttachCylinderRB(LinkNames.UPPER_LINK, JointNames.HIP, robotSide);

      // KNEE ID
      Vector3d kneeOffset = new Vector3d(0.0, 0.0, kneeOffsetZ);
      RevoluteJoint kneeJointID =
          ScrewTools.addRevoluteJoint(
              JointNames.KNEE.getName(), hipJointID.getSuccessor(), kneeOffset, jointAxesPinJoints);
      allLegJoints.get(robotSide).put(JointNames.KNEE, kneeJointID);
      createAndAttachCylinderRB(LinkNames.LOWER_LINK, JointNames.KNEE, robotSide);

      // ANKLE ID (location, joint, and rigidBody)
      Vector3d ankleOffset = new Vector3d(0.0, 0.0, ankleOffsetZ);
      RevoluteJoint ankleJointID =
          ScrewTools.addRevoluteJoint(
              JointNames.ANKLE.getName(),
              kneeJointID.getSuccessor(),
              ankleOffset,
              jointAxesPinJoints);
      allLegJoints.get(robotSide).put(JointNames.ANKLE, ankleJointID);
      createAndAttachFootRB(LinkNames.FOOT_LINK, JointNames.ANKLE, robotSide);
    }

    /** **************** SCS ROBOT ********************** */
    // BODY SCS
    bodyJointSCS = new FloatingPlanarJoint(JointNames.BODY.getName(), this);
    this.addRootJoint(bodyJointSCS);
    createAndAttachBodyLink(LinkNames.BODY_LINK);
    bodyJointSCS.setCartesianPosition(0.0, initialBodyHeight);

    for (RobotSide robotSide : RobotSide.values) {
      // HIP SCS
      PinJoint hipJointSCS =
          new PinJoint(
              robotSide.getSideNameFirstLetter() + JointNames.HIP.getName(),
              new Vector3d(0.0, robotSide.negateIfRightSide(hipOffsetY), 0.0),
              this,
              jointAxesPinJoints);
      hipJointSCS.setLimitStops(-0.75, 0.75, 1e3, 1e1);
      // It is NOT necessary to set limits in the ID description because if the SCS description
      // doesn't let the robot move passed a point the ID robot won't be able to pass it either

      if (robotSide == RobotSide.LEFT) {
        hipJointSCS.setQ(-0.6);
      } else {
        hipJointSCS.setQ(0.1);
      }

      bodyJointSCS.addJoint(hipJointSCS);
      idToSCSLegJointMap.put(allLegJoints.get(robotSide).get(JointNames.HIP), hipJointSCS);
      createAndAttachCylinderLink(LinkNames.UPPER_LINK, JointNames.HIP, robotSide);

      // KNEE SCS
      PinJoint kneeJointSCS =
          new PinJoint(
              robotSide.getSideNameFirstLetter() + JointNames.KNEE.getName(),
              new Vector3d(0.0, 0.0, kneeOffsetZ),
              this,
              jointAxesPinJoints);
      kneeJointSCS.setLimitStops(-0.01, 1.8, 1e5, 1e3);
      hipJointSCS.addJoint(kneeJointSCS);
      idToSCSLegJointMap.put(allLegJoints.get(robotSide).get(JointNames.KNEE), kneeJointSCS);
      createAndAttachCylinderLink(LinkNames.LOWER_LINK, JointNames.KNEE, robotSide);

      // ANKLE SCS
      PinJoint ankleJointSCS =
          new PinJoint(
              robotSide.getSideNameFirstLetter() + JointNames.ANKLE.getName(),
              new Vector3d(0.0, 0.0, ankleOffsetZ),
              this,
              jointAxesPinJoints);
      ankleJointSCS.setLimitStops(-0.7, 0.7, 1e3, 1e2);

      if (robotSide == RobotSide.RIGHT) {
        ankleJointSCS.setQ(-0.1);
      }

      kneeJointSCS.addJoint(ankleJointSCS);
      idToSCSLegJointMap.put(allLegJoints.get(robotSide).get(JointNames.ANKLE), ankleJointSCS);

      // FEET SCS
      createAndAttachFootLink(LinkNames.FOOT_LINK, JointNames.ANKLE, robotSide);
      GroundContactPoint gcHeel =
          new GroundContactPoint(
              robotSide.getSideNameFirstLetter() + "gcHeel",
              new Vector3d(-0.1, 0.0, gcOffset),
              this);
      GCpointsHeel.set(robotSide, gcHeel);
      ankleJointSCS.addGroundContactPoint(gcHeel);
      Graphics3DObject graphicsGCheel = ankleJointSCS.getLink().getLinkGraphics();
      graphicsGCheel.identity();
      graphicsGCheel.translate(-0.1, 0.0, gcOffset);
      graphicsGCheel.addSphere(gcRadius, YoAppearance.Orange());

      if (robotSide == RobotSide.RIGHT) {
        setFStoTrue(robotSide);
      }

      GroundContactPoint gcToe =
          new GroundContactPoint(
              robotSide.getSideNameFirstLetter() + "gcToe", new Vector3d(0.4, 0.0, gcOffset), this);
      GCpointsToe.set(robotSide, gcToe);
      ankleJointSCS.addGroundContactPoint(gcToe);
      Graphics3DObject graphics = ankleJointSCS.getLink().getLinkGraphics();
      graphics.identity();
      graphics.translate(0.4, 0.0, gcOffset);
      graphics.addSphere(gcRadius, YoAppearance.DarkOliveGreen());
    }

    /** ************** SCS Ground Model ************************ */
    GroundContactModel groundModel =
        new LinearGroundContactModel(this, 1e3, 1e3, 5e3, 1e3, this.getRobotsYoVariableRegistry());
    GroundProfile3D profile = new FlatGroundProfile();
    groundModel.setGroundProfile3D(profile);
    this.setGroundContactModel(groundModel);
  }