コード例 #1
0
  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);
  }
  /** *********************** SCS ROBOT - Links ******************************* */
  private void createAndAttachBodyLink(LinkNames linkName) {
    Link link = new Link(LinkNames.BODY_LINK.getName());
    Matrix3d inertiaBody = createInertiaMatrixBox(linkName);
    link.setMomentOfInertia(inertiaBody);
    link.setMass(RobotParameters.MASSES.get(LinkNames.BODY_LINK));
    link.setComOffset(comOffsetBody);

    Graphics3DObject linkGraphics = new Graphics3DObject();
    linkGraphics.addCube(cubeX, cubeY, cubeZ, RobotParameters.APPEARANCE.get(LinkNames.BODY_LINK));
    link.setLinkGraphics(linkGraphics);
    //      link.addEllipsoidFromMassProperties(YoAppearance.Green());
    link.addCoordinateSystemToCOM(0.7);
    bodyJointSCS.setLink(link);
  }
  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);
  }
コード例 #4
0
  private Link body() {

    Link ret = new Link("body");
    ret.setMass(bodyMass);

    // Inertia tensor
    double IxxCube = (bodyMass / 12.0) * (Math.pow(cubeW, 2.0) + Math.pow(cubeL, 2.0));
    double IyyCube = (bodyMass / 12.0) * (Math.pow(cubeH, 2.0) + Math.pow(cubeW, 2.0));
    double IzzCube = (bodyMass / 12.0) * (Math.pow(cubeH, 2.0) + Math.pow(cubeL, 2.0));
    ret.setMomentOfInertia(IxxCube, IyyCube, IzzCube);

    // Graphics
    Graphics3DObject linkGraphics = new Graphics3DObject();
    linkGraphics.addCube(cubeL, cubeW, cubeH, YoAppearance.Glass());

    ret.setLinkGraphics(linkGraphics);
    ret.setComOffset(0.0, 0.0, cubeH / 2.0); // Move the CoM to the center
    // of the body
    ret.addCoordinateSystemToCOM(0.4);

    return ret;
  }