private boolean footLiftedUp(RobotSide robotSide) {
for (GroundContactPoint groundContactPoint : groundContactPointsMap.get(robotSide)) {
if (groundContactPoint.isInContact()) return false;
}
return true;
}
// 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);
}
}
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);
}