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); }