private boolean footLiftedUp(RobotSide robotSide) {
    for (GroundContactPoint groundContactPoint : groundContactPointsMap.get(robotSide)) {
      if (groundContactPoint.isInContact()) return false;
    }

    return true;
  }
コード例 #2
0
  // Override featherstonePassOne since don't need to do everything...
  public void featherstonePassOne(Vector3d w_h, Vector3d v_h, Matrix3d Rh_0) {
    owner.update();
    owner.jointTransform3D.get(Ri_0);

    // this.jointDependentSetAndGetRotation(Ri_0);
    Ri_0.transpose();
    Rh_i = null;
    Ri_h = null;
    d_i = null;
    u_iXd_i = null;
    r_i = null;

    // r_h = null;
    // Now do the joint dependent stuff...
    this.jointDependentFeatherstonePassOne();

    // Now update the points attached to the joint:
    R0_i.set(Ri_0);
    R0_i.transpose();

    // +++JEP OPTIMIZE
    if (groundContactPointGroups != null) {
      Collection<GroundContactPointGroup> groups = groundContactPointGroups.values();
      for (GroundContactPointGroup groundContactPointGroup : groups) {
        ArrayList<GroundContactPoint> groundContactPoints =
            groundContactPointGroup.getGroundContactPointsInContact();

        for (int i = 0; i < groundContactPoints.size(); i++) {
          GroundContactPoint point = groundContactPoints.get(i);

          point.updatePointVelocity(R0_i, owner.link.comOffset, v_i, w_i);
        }
      }
    }

    if (kinematicPoints != null) {
      for (int i = 0; i < kinematicPoints.size(); i++) {
        KinematicPoint point = kinematicPoints.get(i);

        point.updatePointVelocity(R0_i, owner.link.comOffset, v_i, w_i);
      }
    }

    // Recurse over the children:
    for (int i = 0; i < owner.childrenJoints.size(); i++) {
      Joint child = (Joint) owner.childrenJoints.get(i);

      child.physics.featherstonePassOne(w_i, v_i, Ri_0);
    }
  }
コード例 #3
0
  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);
  }