private void addRock(Vector3d normal, double centroidHeight, double[][] vertices) {
ArrayList<Point2d> vertexPoints = new ArrayList<Point2d>();
for (double[] point : vertices) {
Point2d point2d = new Point2d(point);
vertexPoints.add(point2d);
}
ConvexPolygon2d convexPolygon = new ConvexPolygon2d(vertexPoints);
RotatableConvexPolygonTerrainObject rock =
new RotatableConvexPolygonTerrainObject(
normal, convexPolygon, centroidHeight, YoAppearance.Red());
this.combinedTerrainObject.addTerrainObject(rock);
}
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);
}
public CirclePoseTrajectoryGenerator(
String namePrefix,
ReferenceFrame trajectoryFrame,
DoubleProvider trajectoryTimeProvider,
YoVariableRegistry parentRegistry,
YoGraphicsListRegistry yoGraphicsListRegistry) {
this.registry = new YoVariableRegistry(namePrefix + getClass().getSimpleName());
this.desiredTrajectoryTime = new DoubleYoVariable(namePrefix + "TrajectoryTime", registry);
this.currentTime = new DoubleYoVariable(namePrefix + "Time", registry);
// this.anglePolynomial = new YoPolynomial(namePrefix + "ParameterPolynomial", 2,
// registry);
this.anglePolynomial = new YoPolynomial(namePrefix + "ParameterPolynomial", 4, registry);
// this.anglePolynomial = new YoPolynomial(namePrefix + "ParameterPolynomial", 6,
// registry);
this.desiredTrajectoryTimeProvider = trajectoryTimeProvider;
this.trajectoryFrame = trajectoryFrame;
initialRadius = new DoubleYoVariable(namePrefix + "Radius", registry);
initialZ = new DoubleYoVariable(namePrefix + "ZPosition", registry);
initialAngle = new DoubleYoVariable(namePrefix + "InitialAngle", registry);
finalAngle = new DoubleYoVariable(namePrefix + "FinalAngle", registry);
isCurrentAngleBeingAdjusted =
new BooleanYoVariable(namePrefix + "IsCurrentAngleBeingAdjusted", registry);
maximumAngleTrackingErrorTolerated =
new DoubleYoVariable(namePrefix + "MaxAngleTrackingErrorTolerated", registry);
maximumAngleTrackingErrorTolerated.set(Math.toRadians(30.0));
currentControlledFrameRelativeAngle =
new DoubleYoVariable(namePrefix + "CurrentControlledFrameAngle", registry);
currentAngleTrackingError =
new DoubleYoVariable(namePrefix + "CurrentAngleTrackingError", registry);
currentAdjustedRelativeAngle =
new DoubleYoVariable(namePrefix + "CurrentAdjustedRelativeAngle", registry);
desiredRotationAngle = new DoubleYoVariable(namePrefix + "DesiredRotationAngle", registry);
currentRelativeAngle = new DoubleYoVariable(namePrefix + "CurrentRelativeAngle", registry);
yoInitialPosition = new YoFramePoint(namePrefix + "InitialPosition", trajectoryFrame, registry);
yoFinalPosition = new YoFramePoint(namePrefix + "FinalPosition", trajectoryFrame, registry);
yoCurrentPosition = new YoFramePoint(namePrefix + "CurrentPosition", trajectoryFrame, registry);
yoCurrentVelocity =
new YoFrameVector(namePrefix + "CurrentVelocity", trajectoryFrame, registry);
yoCurrentAcceleration =
new YoFrameVector(namePrefix + "CurrentAcceleration", trajectoryFrame, registry);
yoInitialOrientation =
new YoFrameQuaternion(namePrefix + "InitialOrientation", trajectoryFrame, registry);
yoFinalOrientation =
new YoFrameQuaternion(namePrefix + "FinalOrientation", trajectoryFrame, registry);
yoCurrentOrientation =
new YoFrameQuaternion(namePrefix + "CurrentOrientation", trajectoryFrame, registry);
yoCurrentAngularVelocity =
new YoFrameVector(namePrefix + "CurrentAngularVelocity", trajectoryFrame, registry);
yoCurrentAngularAcceleration =
new YoFrameVector(namePrefix + "CurrentAngularAcceleration", trajectoryFrame, registry);
yoInitialPositionWorld =
new YoFramePoint(namePrefix + "InitialPositionWorld", worldFrame, registry);
yoFinalPositionWorld =
new YoFramePoint(namePrefix + "FinalPositionWorld", worldFrame, registry);
yoCurrentPositionWorld =
new YoFramePoint(namePrefix + "CurrentPositionWorld", worldFrame, registry);
yoCurrentAdjustedPositionWorld =
new YoFramePoint(namePrefix + "CurrentAdjustedPositionWorld", worldFrame, registry);
circleOrigin = new YoFramePoint(namePrefix + "CircleOrigin", trajectoryFrame, registry);
rotationAxis = new YoFrameVector(namePrefix + "RotationAxis", trajectoryFrame, registry);
rotationAxis.set(0.0, 0.0, 1.0);
circleFrame =
new ReferenceFrame("CircleFrame", trajectoryFrame) {
private static final long serialVersionUID = 9102217353690768074L;
private final Vector3d localTranslation = new Vector3d();
private final Vector3d localRotationAxis = new Vector3d();
private final AxisAngle4d localAxisAngle = new AxisAngle4d();
@Override
protected void updateTransformToParent(RigidBodyTransform transformToParent) {
circleOrigin.get(localTranslation);
rotationAxis.get(localRotationAxis);
GeometryTools.getRotationBasedOnNormal(localAxisAngle, localRotationAxis);
transformToParent.set(localAxisAngle, localTranslation);
}
};
tangentialCircleFrame = new PoseReferenceFrame("TangentialCircleFrame", circleFrame);
yoTangentialCircleFramePose =
new YoFramePose(namePrefix + "TangentialCircleFramePose", worldFrame, registry);
if (this.visualize && yoGraphicsListRegistry != null) {
final YoGraphicPosition currentPositionViz =
new YoGraphicPosition(
namePrefix + "CurrentPosition", yoCurrentPositionWorld, 0.025, YoAppearance.Blue());
final YoGraphicPosition currentAdjustedPositionViz =
new YoGraphicPosition(
namePrefix + "CurrentAdjustedPosition",
yoCurrentAdjustedPositionWorld,
0.023,
YoAppearance.Gold());
final YoGraphicPosition initialPositionViz =
new YoGraphicPosition(
namePrefix + "InitialPosition",
yoInitialPositionWorld,
0.02,
YoAppearance.BlueViolet());
final YoGraphicPosition finalPositionViz =
new YoGraphicPosition(
namePrefix + "FinalPosition", yoFinalPositionWorld, 0.02, YoAppearance.Red());
final YoGraphicCoordinateSystem tangentialFrameViz =
new YoGraphicCoordinateSystem(
namePrefix + "TangentialFrame",
yoTangentialCircleFramePose,
0.2,
YoAppearance.Pink());
YoGraphicsList yoGraphicsList = new YoGraphicsList(namePrefix + "CircleTraj");
yoGraphicsList.add(currentPositionViz);
yoGraphicsList.add(currentAdjustedPositionViz);
yoGraphicsList.add(initialPositionViz);
yoGraphicsList.add(finalPositionViz);
yoGraphicsList.add(tangentialFrameViz);
yoGraphicsListRegistry.registerYoGraphicsList(yoGraphicsList);
bagOfBalls =
new BagOfBalls(
numberOfBalls, 0.01, yoGraphicsList.getLabel(), registry, yoGraphicsListRegistry);
showViz = new BooleanYoVariable(namePrefix + "ShowViz", registry);
showViz.addVariableChangedListener(
new VariableChangedListener() {
public void variableChanged(YoVariable<?> v) {
boolean visible = showViz.getBooleanValue();
currentPositionViz.setVisible(visible);
currentAdjustedPositionViz.setVisible(visible);
initialPositionViz.setVisible(visible);
finalPositionViz.setVisible(visible);
tangentialFrameViz.setVisible(visible);
bagOfBalls.setVisible(visible);
}
});
showViz.notifyVariableChangedListeners();
} else {
visualize = false;
bagOfBalls = null;
showViz = null;
}
parentRegistry.addChild(registry);
}
public FallingBrickRobot() {
super("FallingBrick");
this.setGravity(0.0, 0.0, -G);
// create the brick as a floating joint
floatingJoint = new FloatingJoint("base", new Vector3d(0.0, 0.0, 0.0), this);
Link link1 = base("base", YoAppearance.Red());
floatingJoint.setLink(link1);
this.addRootJoint(floatingJoint);
// add ground contact points to the brick
GroundContactPoint gc1 =
new GroundContactPoint("gc1", new Vector3d(BASE_L / 2.0, BASE_W / 2.0, BASE_H / 2.0), this);
floatingJoint.addGroundContactPoint(gc1);
GroundContactPoint gc2 =
new GroundContactPoint(
"gc2", new Vector3d(BASE_L / 2.0, -BASE_W / 2.0, BASE_H / 2.0), this);
floatingJoint.addGroundContactPoint(gc2);
GroundContactPoint gc3 =
new GroundContactPoint(
"gc3", new Vector3d(-BASE_L / 2.0, BASE_W / 2.0, BASE_H / 2.0), this);
floatingJoint.addGroundContactPoint(gc3);
GroundContactPoint gc4 =
new GroundContactPoint(
"gc4", new Vector3d(-BASE_L / 2.0, -BASE_W / 2.0, BASE_H / 2.0), this);
floatingJoint.addGroundContactPoint(gc4);
GroundContactPoint gc5 =
new GroundContactPoint(
"gc5", new Vector3d(BASE_L / 2.0, BASE_W / 2.0, -BASE_H / 2.0), this);
floatingJoint.addGroundContactPoint(gc5);
GroundContactPoint gc6 =
new GroundContactPoint(
"gc6", new Vector3d(BASE_L / 2.0, -BASE_W / 2.0, -BASE_H / 2.0), this);
floatingJoint.addGroundContactPoint(gc6);
GroundContactPoint gc7 =
new GroundContactPoint(
"gc7", new Vector3d(-BASE_L / 2.0, BASE_W / 2.0, -BASE_H / 2.0), this);
floatingJoint.addGroundContactPoint(gc7);
GroundContactPoint gc8 =
new GroundContactPoint(
"gc8", new Vector3d(-BASE_L / 2.0, -BASE_W / 2.0, -BASE_H / 2.0), this);
floatingJoint.addGroundContactPoint(gc8);
GroundContactPoint gc9 =
new GroundContactPoint("gc9", new Vector3d(0.0, 0.0, BASE_H / 2.0 + BASE_H), this);
floatingJoint.addGroundContactPoint(gc9);
GroundContactPoint gc10 =
new GroundContactPoint("gc10", new Vector3d(0.0, 0.0, -BASE_H / 2.0 - BASE_H), this);
floatingJoint.addGroundContactPoint(gc10);
this.setController(this); // tells the simulator to call the local doControl() method
// instantiate ground contact model
GroundContactModel groundModel =
new LinearGroundContactModel(
this, 1422, 150.6, 50.0, 1000.0, this.getRobotsYoVariableRegistry());
// GroundContactModel groundModel = new CollisionGroundContactModel(this, 0.5, 0.7);
GroundProfile3D profile = new WavyGroundProfile();
groundModel.setGroundProfile3D(profile);
this.setGroundContactModel(groundModel);
initRobot();
initControl();
}
