private CombinedTerrainObject3D setUpGround(String name) {
CombinedTerrainObject3D combinedTerrainObject = new CombinedTerrainObject3D(name);
combinedTerrainObject.addBox(-10.0, -10.0, 10.0, 10.0, -0.05, 0.0, YoAppearance.DarkGray());
combinedTerrainObject.addBox(2.0, -0.05, 3.0, 0.05, 2.0, YoAppearance.Beige());
combinedTerrainObject.addBox(
3.0 + ContactableDoorRobot.DEFAULT_DOOR_DIMENSIONS.x,
-0.05,
4.0 + ContactableDoorRobot.DEFAULT_DOOR_DIMENSIONS.x,
0.05,
2.0,
YoAppearance.Beige());
return combinedTerrainObject;
}
private CombinedTerrainObject3D setUpGround(String name) {
CombinedTerrainObject3D combinedTerrainObject = new CombinedTerrainObject3D(name);
combinedTerrainObject.addBox(-10.0, -10.0, 10.0, 10.0, -0.05, 0.0, YoAppearance.DarkBlue());
return combinedTerrainObject;
}
public void addVerticalDebrisLeaningAgainstAWall(
double xRelativeToRobot, double yRelativeToRobot, double debrisYaw, double debrisPitch) {
Point3d tempPosition =
new Point3d(xRelativeToRobot, yRelativeToRobot, debrisLength / 2.0 * Math.cos(debrisPitch));
FramePose debrisPose = generateDebrisPose(tempPosition, debrisYaw, debrisPitch, 0.0);
debrisRobots.add(createDebrisRobot(debrisPose));
double supportWidth = 0.1;
double supportLength = 0.2;
double supportHeight = 1.05 * debrisLength;
RigidBodyTransform debrisTransform = new RigidBodyTransform();
debrisPose.getPose(debrisTransform);
TransformTools.rotate(debrisTransform, -debrisPitch, Axis.Y);
debrisPose.setPose(debrisTransform);
debrisPose.setZ(0.0);
PoseReferenceFrame debrisReferenceFrame =
new PoseReferenceFrame("debrisReferenceFrame", debrisPose);
FramePose supportPose = new FramePose(debrisReferenceFrame);
double x = supportWidth / 2.0 + debrisLength / 2.0 * Math.sin(debrisPitch);
double y = 0.0;
double z = supportHeight / 2.0;
supportPose.setPosition(x, y, z);
supportPose.changeFrame(constructionWorldFrame);
RigidBodyTransform supportTransform = new RigidBodyTransform();
supportPose.getPose(supportTransform);
combinedTerrainObject.addRotatableBox(
supportTransform, supportWidth, supportLength, supportHeight, YoAppearance.AliceBlue());
}
/** Joints */
public Step2Robot() {
super("v2Robot");
this.setGravity(-9.81);
// Body (2DOF = Z + Pitch)
bodyJoint1 = new SliderJoint("bodyZ", new Vector3d(0.0, 0.0, 0.0), this, Axis.Z);
bodyJoint1.setDynamic(true);
Link bodyLinkSlider = setNullLink();
bodyJoint1.setLink(bodyLinkSlider);
this.addRootJoint(bodyJoint1);
bodyJoint1.setInitialState(legHeight, 0.0);
bodyJoint2 = new PinJoint("bodyPitch", new Vector3d(0.0, 0.0, 0.0), this, Axis.Y);
bodyJoint2.setDynamic(true);
Link bodyLinkPitch = body();
bodyJoint2.setLink(bodyLinkPitch);
bodyJoint1.addJoint(bodyJoint2);
// Upper Joint
hipJoint = new PinJoint("hip", new Vector3d(0.0, 0.0, 0.0), this, Axis.Y);
hipJoint.setDynamic(true);
hipJoint.setLimitStops(0.0, 1.0, 1e6, 1e3);
Link upperLink = upperLink();
hipJoint.setLink(upperLink);
bodyJoint2.addJoint(hipJoint);
// Lower Joint
kneeJoint = new SliderJoint("knee", new Vector3d(0.0, 0.0, -upperLinkLength), this, Axis.Z);
kneeJoint.setDynamic(true);
kneeJoint.setLimitStops(0.0, 0.6, 1e9, 1e2);
Link lowerLink = lowerLink();
kneeJoint.setLink(lowerLink);
hipJoint.addJoint(kneeJoint);
// Visible ground contact point
GroundContactPoint contactPoint =
new GroundContactPoint("Foot", new Vector3d(0.0, 0.0, gcOffset), this);
kneeJoint.addGroundContactPoint(contactPoint);
Graphics3DObject graphics = kneeJoint.getLink().getLinkGraphics();
graphics.identity();
graphics.translate(0.0, 0.0, gcOffset);
double radius = 0.03;
graphics.addSphere(radius, YoAppearance.Orange());
// Ground Model
GroundContactModel groundModel =
new LinearGroundContactModel(
this, 1500, 150, 50000.0, 1e5, this.getRobotsYoVariableRegistry());
GroundProfile3D profile = new FlatGroundProfile();
groundModel.setGroundProfile3D(profile);
this.setGroundContactModel(groundModel);
}
public DoublePendulum() {
super("DoublePendulum");
j1 = new PinJoint("j1", new Vector3d(0, 0, 2), this, Axis.X);
Link l1 = new Link("l1");
l1.setComOffset(0, 0, 0.5);
l1.setMassAndRadiiOfGyration(1.0, 0.05, 0.05, 0.3);
l1.addEllipsoidFromMassProperties(YoAppearance.Pink());
j1.setLink(l1);
j2 = new PinJoint("j2", new Vector3d(0.0, 0.0, 1.0), this, Axis.X);
Link l2 = new Link("l2");
l2.setComOffset(0, 0, 0.5);
l2.setMassAndRadiiOfGyration(1.0, 0.05, 0.05, 0.3);
l2.addEllipsoidFromMassProperties(YoAppearance.Purple());
j2.setLink(l2);
j1.addJoint(j2);
addRootJoint(j1);
}
private Link ball() {
Link ret = new Link("ball");
ret.setMass(M1);
ret.setMomentOfInertia(Ixx1, Iyy1, Izz1);
ret.setComOffset(0.0, 0.0, 0.0);
Graphics3DObject linkGraphics = new Graphics3DObject();
linkGraphics.addSphere(R / 2.0, YoAppearance.EarthTexture());
ret.setLinkGraphics(linkGraphics);
return ret;
}
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);
}
private Link lowerLink() {
Link ret = new Link("lowerLink");
ret.setMass(lowerLinkMass);
// Inertia tensor
double IxxCyl = (lowerLinkMass / 3) * (Math.pow(lowerLinkLength, 2.0));
double IzzCyl = IxxCyl;
ret.setMomentOfInertia(IxxCyl, 0.0, IzzCyl);
// Graphics
Graphics3DObject linkGraphics = new Graphics3DObject();
linkGraphics.addCylinder(-lowerLinkLength, lowerLinkRadius, YoAppearance.Glass());
ret.setLinkGraphics(linkGraphics);
ret.setComOffset(0.0, 0.0, -lowerLinkLength / 2.0);
ret.addCoordinateSystemToCOM(0.4);
return ret;
}
private void addWall() {
Vector3d normal = new Vector3d(0.0, 0.0, 1.0);
double centroidHeight = 2.0;
ArrayList<Point2d> pointList = new ArrayList<Point2d>();
Point2d wallPoint0 = new Point2d(WALL_START_X, WALL_Y);
Point2d wallPoint1 = new Point2d(WALL_START_X + WALL_LENGTH, WALL_Y);
Point2d wallPoint2 =
new Point2d(WALL_START_X + WALL_LENGTH, WALL_Y + Math.signum(WALL_Y) * WALL_THICKNESS);
Point2d wallPoint3 = new Point2d(WALL_START_X, WALL_Y + Math.signum(WALL_Y) * WALL_THICKNESS);
pointList.add(wallPoint0);
pointList.add(wallPoint1);
pointList.add(wallPoint2);
pointList.add(wallPoint3);
ConvexPolygon2d convexPolygon = new ConvexPolygon2d(pointList);
RotatableConvexPolygonTerrainObject rightWall =
new RotatableConvexPolygonTerrainObject(
normal, convexPolygon, centroidHeight, YoAppearance.Brown());
combinedTerrainObject.addTerrainObject(rightWall);
}
private void addPillars() {
Vector3d normal = new Vector3d(0.0, 0.0, 1.0);
double centroidHeight = 2.0;
Point2d bottomLeft = new Point2d(-PILLAR_WIDTH / 2.0, PILLAR_WIDTH / 2.0);
Point2d bottomRight = new Point2d(-PILLAR_WIDTH / 2.0, -PILLAR_WIDTH / 2.0);
Point2d topLeft = new Point2d(PILLAR_WIDTH / 2.0, PILLAR_WIDTH / 2.0);
Point2d topRight = new Point2d(PILLAR_WIDTH / 2.0, -PILLAR_WIDTH / 2.0);
double pillarDistance = WALL_LENGTH / (NUM_PILLARS - 1.0);
Vector2d offset = new Vector2d(0.0, -WALL_Y + PILLAR_WIDTH / 2.0);
for (int i = 0; i < NUM_PILLARS; i++) {
ArrayList<Point2d> points = new ArrayList<Point2d>();
offset.setX(WALL_START_X + pillarDistance * i);
Point2d localBottomLeft = new Point2d();
localBottomLeft.add(bottomLeft, offset);
Point2d localBottomRight = new Point2d();
localBottomRight.add(bottomRight, offset);
Point2d localTopLeft = new Point2d();
localTopLeft.add(topLeft, offset);
Point2d localTopRight = new Point2d();
localTopRight.add(topRight, offset);
points.add(localBottomLeft);
points.add(localBottomRight);
points.add(localTopLeft);
points.add(localTopRight);
ConvexPolygon2d convexPolygon = new ConvexPolygon2d(points);
AppearanceDefinition appearance = YoAppearance.Brown();
// YoAppearance.makeTransparent(appearance, 0.7f);
RotatableConvexPolygonTerrainObject pillar =
new RotatableConvexPolygonTerrainObject(
normal, convexPolygon, centroidHeight, appearance);
combinedTerrainObject.addTerrainObject(pillar);
}
}
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;
}
public TwoWaypointSwingGenerator(
String namePrefix,
double maxSwingHeight,
YoVariableRegistry parentRegistry,
YoGraphicsListRegistry yoGraphicsListRegistry) {
registry = new YoVariableRegistry(namePrefix + getClass().getSimpleName());
parentRegistry.addChild(registry);
stepTime = new DoubleYoVariable(namePrefix + "StepTime", registry);
timeIntoStep = new DoubleYoVariable(namePrefix + "TimeIntoStep", registry);
isDone = new BooleanYoVariable(namePrefix + "IsDone", registry);
trajectoryType =
new EnumYoVariable<>(namePrefix + "TrajectoryType", registry, TrajectoryType.class);
swingHeight = new DoubleYoVariable(namePrefix + "SwingHeight", registry);
swingHeight.set(defaultSwingHeight);
this.maxSwingHeight = new DoubleYoVariable(namePrefix + "MaxSwingHeight", registry);
this.maxSwingHeight.set(maxSwingHeight);
trajectory =
new Position2dOptimizedSwingTrajectoryGenerator(
namePrefix, registry, yoGraphicsListRegistry, maxTimeIterations);
for (int i = 0; i < numberWaypoints; i++) waypointPositions.add(new FramePoint());
if (yoGraphicsListRegistry != null)
waypointViz =
new BagOfBalls(
numberWaypoints,
0.02,
namePrefix + "Waypoints",
YoAppearance.White(),
registry,
yoGraphicsListRegistry);
else waypointViz = null;
}
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 void addHorizontalDebrisLeaningOnTwoBoxes(
Point3d positionOfCenterOfDebrisWithRespectToRobot, double debrisYaw, double debrisRoll) {
double supportWidth = 0.1;
double supportLength = 0.2;
double supportHeight;
double x;
double y;
double z;
FramePose debrisPose =
generateDebrisPose(positionOfCenterOfDebrisWithRespectToRobot, debrisYaw, 0.0, debrisRoll);
debrisRobots.add(createDebrisRobot(debrisPose));
RigidBodyTransform debrisTransform = new RigidBodyTransform();
debrisPose.getPose(debrisTransform);
TransformTools.rotate(debrisTransform, -debrisRoll, Axis.X);
debrisPose.setPose(debrisTransform);
debrisPose.setZ(0.0);
PoseReferenceFrame debrisReferenceFrame =
new PoseReferenceFrame("debrisReferenceFrame", debrisPose);
// add first support
FramePose firstSupportPose = new FramePose(debrisReferenceFrame);
supportHeight =
positionOfCenterOfDebrisWithRespectToRobot.getZ()
- debrisWidth / 2.0
+ debrisLength / 2.0 * Math.cos(debrisRoll);
x = 0.0;
y = -debrisLength / 2.0 * Math.sin(debrisRoll);
z = supportHeight / 2.0;
firstSupportPose.setPosition(x, y, z);
firstSupportPose.changeFrame(constructionWorldFrame);
RigidBodyTransform firstSupportTransform = new RigidBodyTransform();
firstSupportPose.getPose(firstSupportTransform);
combinedTerrainObject.addRotatableBox(
firstSupportTransform,
supportLength,
supportWidth,
supportHeight,
YoAppearance.AliceBlue());
// add second support
FramePose secondSupportPose = new FramePose(debrisReferenceFrame);
supportHeight =
positionOfCenterOfDebrisWithRespectToRobot.getZ()
- debrisWidth / 2.0
- debrisLength / 2.0 * Math.cos(debrisRoll);
x = 0.0;
y = debrisLength / 2.0 * Math.sin(debrisRoll);
z = supportHeight / 2.0;
secondSupportPose.setPosition(x, y, z);
secondSupportPose.changeFrame(constructionWorldFrame);
RigidBodyTransform secondSupportTransform = new RigidBodyTransform();
secondSupportPose.getPose(secondSupportTransform);
combinedTerrainObject.addRotatableBox(
secondSupportTransform,
supportLength,
supportWidth,
supportHeight,
YoAppearance.AliceBlue());
}
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);
}
/** Joints */
public Step6IDandSCSRobot_pinKnee() {
super("Robot");
/** **************** ID ROBOT ********************** */
elevator = new RigidBody("elevator", elevatorFrame);
bodyJointID = new SixDoFJoint(JointNames.BODY.getName(), elevator, elevatorFrame);
createAndAttachBodyRB(LinkNames.BODY_LINK, bodyJointID);
for (RobotSide robotSide : RobotSide.values) {
// HIP ID (location, joint, and rigidBody)
Vector3d hipOffset = new Vector3d(0.0, robotSide.negateIfRightSide(hipOffsetY), 0.0);
RevoluteJoint hipJointID =
ScrewTools.addRevoluteJoint(
JointNames.HIP.getName(),
bodyJointID.getSuccessor(),
hipOffset,
jointAxesPinJoints); // The parent rigid body of the hip joint is:
// bodyJointID.getSuccessor()
allLegJoints.get(robotSide).put(JointNames.HIP, hipJointID);
createAndAttachCylinderRB(LinkNames.UPPER_LINK, JointNames.HIP, robotSide);
// KNEE ID
Vector3d kneeOffset = new Vector3d(0.0, 0.0, kneeOffsetZ);
RevoluteJoint kneeJointID =
ScrewTools.addRevoluteJoint(
JointNames.KNEE.getName(), hipJointID.getSuccessor(), kneeOffset, jointAxesPinJoints);
allLegJoints.get(robotSide).put(JointNames.KNEE, kneeJointID);
createAndAttachCylinderRB(LinkNames.LOWER_LINK, JointNames.KNEE, robotSide);
// ANKLE ID (location, joint, and rigidBody)
Vector3d ankleOffset = new Vector3d(0.0, 0.0, ankleOffsetZ);
RevoluteJoint ankleJointID =
ScrewTools.addRevoluteJoint(
JointNames.ANKLE.getName(),
kneeJointID.getSuccessor(),
ankleOffset,
jointAxesPinJoints);
allLegJoints.get(robotSide).put(JointNames.ANKLE, ankleJointID);
createAndAttachFootRB(LinkNames.FOOT_LINK, JointNames.ANKLE, robotSide);
}
/** **************** SCS ROBOT ********************** */
// BODY SCS
bodyJointSCS = new FloatingPlanarJoint(JointNames.BODY.getName(), this);
this.addRootJoint(bodyJointSCS);
createAndAttachBodyLink(LinkNames.BODY_LINK);
bodyJointSCS.setCartesianPosition(0.0, initialBodyHeight);
for (RobotSide robotSide : RobotSide.values) {
// HIP SCS
PinJoint hipJointSCS =
new PinJoint(
robotSide.getSideNameFirstLetter() + JointNames.HIP.getName(),
new Vector3d(0.0, robotSide.negateIfRightSide(hipOffsetY), 0.0),
this,
jointAxesPinJoints);
hipJointSCS.setLimitStops(-0.75, 0.75, 1e3, 1e1);
// It is NOT necessary to set limits in the ID description because if the SCS description
// doesn't let the robot move passed a point the ID robot won't be able to pass it either
if (robotSide == RobotSide.LEFT) {
hipJointSCS.setQ(-0.6);
} else {
hipJointSCS.setQ(0.1);
}
bodyJointSCS.addJoint(hipJointSCS);
idToSCSLegJointMap.put(allLegJoints.get(robotSide).get(JointNames.HIP), hipJointSCS);
createAndAttachCylinderLink(LinkNames.UPPER_LINK, JointNames.HIP, robotSide);
// KNEE SCS
PinJoint kneeJointSCS =
new PinJoint(
robotSide.getSideNameFirstLetter() + JointNames.KNEE.getName(),
new Vector3d(0.0, 0.0, kneeOffsetZ),
this,
jointAxesPinJoints);
kneeJointSCS.setLimitStops(-0.01, 1.8, 1e5, 1e3);
hipJointSCS.addJoint(kneeJointSCS);
idToSCSLegJointMap.put(allLegJoints.get(robotSide).get(JointNames.KNEE), kneeJointSCS);
createAndAttachCylinderLink(LinkNames.LOWER_LINK, JointNames.KNEE, robotSide);
// ANKLE SCS
PinJoint ankleJointSCS =
new PinJoint(
robotSide.getSideNameFirstLetter() + JointNames.ANKLE.getName(),
new Vector3d(0.0, 0.0, ankleOffsetZ),
this,
jointAxesPinJoints);
ankleJointSCS.setLimitStops(-0.7, 0.7, 1e3, 1e2);
if (robotSide == RobotSide.RIGHT) {
ankleJointSCS.setQ(-0.1);
}
kneeJointSCS.addJoint(ankleJointSCS);
idToSCSLegJointMap.put(allLegJoints.get(robotSide).get(JointNames.ANKLE), ankleJointSCS);
// FEET SCS
createAndAttachFootLink(LinkNames.FOOT_LINK, JointNames.ANKLE, robotSide);
GroundContactPoint gcHeel =
new GroundContactPoint(
robotSide.getSideNameFirstLetter() + "gcHeel",
new Vector3d(-0.1, 0.0, gcOffset),
this);
GCpointsHeel.set(robotSide, gcHeel);
ankleJointSCS.addGroundContactPoint(gcHeel);
Graphics3DObject graphicsGCheel = ankleJointSCS.getLink().getLinkGraphics();
graphicsGCheel.identity();
graphicsGCheel.translate(-0.1, 0.0, gcOffset);
graphicsGCheel.addSphere(gcRadius, YoAppearance.Orange());
if (robotSide == RobotSide.RIGHT) {
setFStoTrue(robotSide);
}
GroundContactPoint gcToe =
new GroundContactPoint(
robotSide.getSideNameFirstLetter() + "gcToe", new Vector3d(0.4, 0.0, gcOffset), this);
GCpointsToe.set(robotSide, gcToe);
ankleJointSCS.addGroundContactPoint(gcToe);
Graphics3DObject graphics = ankleJointSCS.getLink().getLinkGraphics();
graphics.identity();
graphics.translate(0.4, 0.0, gcOffset);
graphics.addSphere(gcRadius, YoAppearance.DarkOliveGreen());
}
/** ************** SCS Ground Model ************************ */
GroundContactModel groundModel =
new LinearGroundContactModel(this, 1e3, 1e3, 5e3, 1e3, this.getRobotsYoVariableRegistry());
GroundProfile3D profile = new FlatGroundProfile();
groundModel.setGroundProfile3D(profile);
this.setGroundContactModel(groundModel);
}
public class ClassicCameraController
implements TrackingDollyCameraController,
KeyListener,
MouseListener,
Mouse3DListener,
SelectedListener {
public static final double MIN_FIELD_OF_VIEW = 0.001;
public static final double MAX_FIELD_OF_VIEW = 2.0;
private static final double MIN_CAMERA_POSITION_TO_FIX_DISTANCE = 0.1; // 0.8;
public static final double CAMERA_START_X = 0.0;
public static final double CAMERA_START_Y = -6.0;
public static final double CAMERA_START_Z = 1.0;
private double fieldOfView = CameraConfiguration.DEFAULT_FIELD_OF_VIEW;
private double clipDistanceNear = CameraConfiguration.DEFAULT_CLIP_DISTANCE_NEAR;
private double clipDistanceFar = CameraConfiguration.DEFAULT_CLIP_DISTANCE_FAR;
private double camX, camY, camZ, fixX, fixY, fixZ;
private double zoom_factor = 1.0;
private double rotate_factor = 1.0;
private double rotate_camera_factor = 1.0;
private boolean isMounted = false;
private CameraMountInterface cameraMount;
private boolean isTracking = true, isTrackingX = true, isTrackingY = true, isTrackingZ = false;
private boolean isDolly = false, isDollyX = true, isDollyY = true, isDollyZ = false;
private double trackDX = 0.0, trackDY = 0.0, trackDZ = 0.0;
private double dollyDX = 2.0, dollyDY = 12.0, dollyDZ = 0.0;
private ViewportAdapter viewportAdapter;
private JFrame jFrame;
// Flying
private boolean fly = true;
private boolean forward = false;
private boolean backward = false;
private boolean left = false;
private boolean right = false;
private boolean up = false;
private boolean down = false;
private ArrayList<Point3d> storedCameraPositions = new ArrayList<Point3d>(0);
private ArrayList<Point3d> storedFixPositions = new ArrayList<Point3d>(0);
private int storedPositionIndex = 0;
private boolean transitioning = false;
private static double transitionTime = 500;
private double camXSpeed;
private double camYSpeed;
private double camZSpeed;
private double fixXSpeed;
private double fixYSpeed;
private double fixZSpeed;
private long lastTransitionTime;
private ArrayList<Point3d> keyFrameCamPos = new ArrayList<Point3d>(0);
private ArrayList<Point3d> keyFrameFixPos = new ArrayList<Point3d>(0);
private ArrayList<Integer> keyFrameTimes = new ArrayList<Integer>(0);
private Graphics3DNode fixPointNode =
new Graphics3DNode(
"cameraFixPoint",
new Graphics3DObject(new Sphere3d(0.01), YoAppearance.RGBColor(1.0, 0.0, 0.0, 0.5)));
private boolean toggleCameraKeyPoints = false;
private int cameraKeyPointIndex;
private ArrayList<Integer> cameraKeyPoints = new ArrayList<Integer>(0);
private CameraTrackingAndDollyPositionHolder cameraTrackAndDollyVariablesHolder;
private Graphics3DAdapter graphics3dAdapter;
public static ClassicCameraController createClassicCameraControllerAndAddListeners(
ViewportAdapter viewportAdapter,
CameraTrackingAndDollyPositionHolder cameraTrackAndDollyVariablesHolder,
Graphics3DAdapter graphics3dAdapter) {
return createClassicCameraControllerAndAddListeners(
viewportAdapter, cameraTrackAndDollyVariablesHolder, graphics3dAdapter, null);
}
public static ClassicCameraController createClassicCameraControllerAndAddListeners(
ViewportAdapter viewportAdapter,
CameraTrackingAndDollyPositionHolder cameraTrackAndDollyVariablesHolder,
Graphics3DAdapter graphics3dAdapter,
JFrame jFrame) {
ClassicCameraController classicCameraController =
new ClassicCameraController(
graphics3dAdapter, viewportAdapter, cameraTrackAndDollyVariablesHolder, jFrame);
graphics3dAdapter.addKeyListener(classicCameraController);
graphics3dAdapter.addMouseListener(classicCameraController);
graphics3dAdapter.addMouse3DListener(classicCameraController);
graphics3dAdapter.addSelectedListener(classicCameraController);
return classicCameraController;
}
public ClassicCameraController(
Graphics3DAdapter graphics3dAdapter,
ViewportAdapter viewportAdapter,
CameraTrackingAndDollyPositionHolder cameraTrackAndDollyVariablesHolder) {
this(graphics3dAdapter, viewportAdapter, cameraTrackAndDollyVariablesHolder, null);
}
public ClassicCameraController(
Graphics3DAdapter graphics3dAdapter,
ViewportAdapter viewportAdapter,
CameraTrackingAndDollyPositionHolder cameraTrackAndDollyVariablesHolder,
JFrame jFrame) {
if (graphics3dAdapter == null) throw new RuntimeException("graphics3dAdapter == null");
this.graphics3dAdapter = graphics3dAdapter;
this.viewportAdapter = viewportAdapter;
this.jFrame = jFrame;
this.camX = CAMERA_START_X;
this.camY = CAMERA_START_Y;
this.camZ = CAMERA_START_Z;
this.fixX = 0.0;
this.fixY = 0.0;
this.fixZ = 0.6;
this.cameraTrackAndDollyVariablesHolder = cameraTrackAndDollyVariablesHolder;
// Don't do this stuff by default
setTracking(false);
setDolly(false);
}
public void setCameraMount(CameraMountInterface mount) {
this.cameraMount = mount;
}
public CameraMountInterface getCameraMount() {
return cameraMount;
}
public boolean isMounted() {
return isMounted;
}
@Override
public CameraTrackingAndDollyPositionHolder getCameraTrackAndDollyVariablesHolder() {
return cameraTrackAndDollyVariablesHolder;
}
@Override
public void setConfiguration(CameraConfiguration config, CameraMountList mountList) {
if (config == null) return;
this.isMounted = config.isCameraMounted();
if (isMounted && (mountList != null)) {
this.cameraMount = mountList.getCameraMount(config.getCameraMountName());
}
this.camX = config.camX;
this.camY = config.camY;
this.camZ = config.camZ;
this.fixX = config.fixX;
this.fixY = config.fixY;
this.fixZ = config.fixZ;
this.isTracking = config.isTracking;
this.isTrackingX = config.isTrackingX;
this.isTrackingY = config.isTrackingY;
this.isTrackingZ = config.isTrackingZ;
this.isDolly = config.isDolly;
this.isDollyX = config.isDollyX;
this.isDollyY = config.isDollyY;
this.isDollyZ = config.isDollyZ;
this.trackDX = config.trackDX;
this.trackDY = config.trackDY;
this.trackDZ = config.trackDZ;
this.dollyDX = config.dollyDX;
this.dollyDY = config.dollyDY;
this.dollyDZ = config.dollyDZ;
setFieldOfView(config.fieldOfView);
this.clipDistanceFar = config.clipDistanceFar;
this.clipDistanceNear = config.clipDistanceNear;
// this.update();
}
@Override
public boolean isTracking() {
return isTracking;
}
@Override
public boolean isTrackingX() {
return isTrackingX;
}
@Override
public boolean isTrackingY() {
return isTrackingY;
}
@Override
public boolean isTrackingZ() {
return isTrackingZ;
}
@Override
public boolean isDolly() {
return isDolly;
}
@Override
public boolean isDollyX() {
return isDollyX;
}
@Override
public boolean isDollyY() {
return isDollyY;
}
@Override
public boolean isDollyZ() {
return isDollyZ;
}
@Override
public void setTracking(boolean track, boolean trackX, boolean trackY, boolean trackZ) {
setTracking(track);
setTrackingX(trackX);
setTrackingY(trackY);
setTrackingZ(trackZ);
}
@Override
public void setDolly(boolean dolly, boolean dollyX, boolean dollyY, boolean dollyZ) {
setDolly(dolly);
setDollyX(dollyX);
setDollyY(dollyY);
setDollyZ(dollyZ);
}
@Override
public void setTrackingOffsets(double dx, double dy, double dz) {
trackDX = dx;
trackDY = dy;
trackDZ = dz;
}
@Override
public void setDollyOffsets(double dx, double dy, double dz) {
dollyDX = dx;
dollyDY = dy;
dollyDZ = dz;
}
@Override
public void setTracking(boolean track) {
isTracking = track;
}
@Override
public void setTrackingX(boolean trackX) {
isTrackingX = trackX;
}
@Override
public void setTrackingY(boolean trackY) {
isTrackingY = trackY;
}
@Override
public void setTrackingZ(boolean trackZ) {
isTrackingZ = trackZ;
}
@Override
public void setDolly(boolean dolly) {
isDolly = dolly;
}
@Override
public void setDollyX(boolean dollyX) {
isDollyX = dollyX;
}
@Override
public void setDollyY(boolean dollyY) {
isDollyY = dollyY;
}
@Override
public void setDollyZ(boolean dollyZ) {
isDollyZ = dollyZ;
}
@Override
public double getTrackingXOffset() {
return trackDX;
}
@Override
public double getTrackingYOffset() {
return trackDY;
}
@Override
public double getTrackingZOffset() {
return trackDZ;
}
@Override
public double getDollyXOffset() {
return dollyDX;
}
@Override
public double getDollyYOffset() {
return dollyDY;
}
@Override
public double getDollyZOffset() {
return dollyDZ;
}
@Override
public void setTrackingXOffset(double dx) {
trackDX = dx;
}
@Override
public void setTrackingYOffset(double dy) {
trackDY = dy;
}
@Override
public void setTrackingZOffset(double dz) {
trackDZ = dz;
}
@Override
public void setDollyXOffset(double dx) {
dollyDX = dx;
}
@Override
public void setDollyYOffset(double dy) {
dollyDY = dy;
}
@Override
public void setDollyZOffset(double dz) {
dollyDZ = dz;
}
@Override
public void update() {
if (graphics3dAdapter.getContextManager().getCurrentViewport() != viewportAdapter) {
forward = false;
backward = false;
left = false;
right = false;
up = false;
down = false;
}
if (isTracking) {
if (isTrackingX) {
double trackX = cameraTrackAndDollyVariablesHolder.getTrackingX();
if (!Double.isNaN(trackX)) fixX = trackX + trackDX;
}
if (isTrackingY) {
double trackY = cameraTrackAndDollyVariablesHolder.getTrackingY();
if (!Double.isNaN(trackY)) fixY = trackY + trackDY;
}
if (isTrackingZ) {
double trackZ = cameraTrackAndDollyVariablesHolder.getTrackingZ();
if (!Double.isNaN(trackZ)) fixZ = trackZ + trackDZ;
}
}
if (isDolly) {
double dollyX = cameraTrackAndDollyVariablesHolder.getDollyX();
if (isDollyX) {
if (!Double.isNaN(dollyX)) camX = dollyX + dollyDX;
}
if (isDollyY) {
double dollyY = cameraTrackAndDollyVariablesHolder.getDollyY();
if (!Double.isNaN(dollyY)) camY = dollyY + dollyDY;
}
if (isDollyZ) {
double dollyZ = cameraTrackAndDollyVariablesHolder.getDollyZ();
if (!Double.isNaN(dollyZ)) camZ = dollyZ + dollyDZ;
}
}
double fieldOfView = cameraTrackAndDollyVariablesHolder.getFieldOfView();
if (!Double.isNaN(fieldOfView)) setFieldOfView(fieldOfView);
// Flying
if (fly && !isTracking && !isDolly && !transitioning) {
if (forward) {
moveCameraForward(-0.5);
}
if (backward) {
moveCameraForward(0.5);
}
if (left) {
pan(20, 0);
}
if (right) {
pan(-20, 0);
}
if (up) {
pan(00, 20);
}
if (down) {
pan(00, -20);
}
}
// End Flying
if (transitioning && !isTracking && !isDolly) {
int numberOfDimensionsThatHaveTransitioned = 0;
double elapsedTransitionTime = System.currentTimeMillis() - lastTransitionTime;
lastTransitionTime = System.currentTimeMillis();
if (Math.abs(camX - storedCameraPositions.get(storedPositionIndex).getX())
<= Math.abs(camXSpeed * elapsedTransitionTime)) {
camX = storedCameraPositions.get(storedPositionIndex).getX();
numberOfDimensionsThatHaveTransitioned++;
} else {
camX += camXSpeed * elapsedTransitionTime;
}
if (Math.abs(camY - storedCameraPositions.get(storedPositionIndex).getY())
<= Math.abs(camYSpeed * elapsedTransitionTime)) {
camY = storedCameraPositions.get(storedPositionIndex).getY();
numberOfDimensionsThatHaveTransitioned++;
} else {
camY += camYSpeed * elapsedTransitionTime;
}
if (Math.abs(camZ - storedCameraPositions.get(storedPositionIndex).getZ())
<= Math.abs(camZSpeed * elapsedTransitionTime)) {
camZ = storedCameraPositions.get(storedPositionIndex).getZ();
numberOfDimensionsThatHaveTransitioned++;
} else {
camZ += camZSpeed * elapsedTransitionTime;
}
if (Math.abs(fixX - storedFixPositions.get(storedPositionIndex).getX())
<= Math.abs(fixXSpeed * elapsedTransitionTime)) {
fixX = storedFixPositions.get(storedPositionIndex).getX();
numberOfDimensionsThatHaveTransitioned++;
} else {
fixX += fixXSpeed * elapsedTransitionTime;
}
if (Math.abs(fixY - storedFixPositions.get(storedPositionIndex).getY())
<= Math.abs(fixYSpeed * elapsedTransitionTime)) {
fixY = storedFixPositions.get(storedPositionIndex).getY();
numberOfDimensionsThatHaveTransitioned++;
} else {
fixY += fixYSpeed * elapsedTransitionTime;
}
if (Math.abs(fixZ - storedFixPositions.get(storedPositionIndex).getZ())
<= Math.abs(fixZSpeed * elapsedTransitionTime)) {
fixZ = storedFixPositions.get(storedPositionIndex).getZ();
numberOfDimensionsThatHaveTransitioned++;
} else {
fixZ += fixZSpeed * elapsedTransitionTime;
}
if (numberOfDimensionsThatHaveTransitioned == 6) {
transitioning = false;
}
}
}
public void addKeyFrame(int time) {
addKeyFrame(keyFrameCamPos.size(), time);
}
public void addKeyFrame(int i, int time) {
keyFrameCamPos.add(i, new Point3d(camX, camY, camZ));
keyFrameFixPos.add(i, new Point3d(fixX, fixY, fixZ));
keyFrameTimes.add(i, time);
}
public int removeKeyFrameByTime(int time) {
for (int i = 0; i < keyFrameTimes.size(); i++) {
if (keyFrameTimes.get(i) == time) {
removeKeyFrameByIndex(i);
return i;
}
}
return -1;
}
public void removeKeyFrameByIndex(int i) {
if ((i >= 0) && (i < keyFrameTimes.size())) {
keyFrameTimes.remove(i);
keyFrameCamPos.remove(i);
keyFrameFixPos.remove(i);
}
}
@Override
public void setKeyFrameTime(int time) {
for (int i = keyFrameTimes.size() - 1; i >= 0; i--) {
if (time >= keyFrameTimes.get(i)) {
if (keyFrameTimes.size() > i + 1) {
double elapsedTime = time - keyFrameTimes.get(i);
double totalTime = keyFrameTimes.get(i + 1) - keyFrameTimes.get(i);
camX =
keyFrameCamPos.get(i).getX()
+ (keyFrameCamPos.get(i + 1).getX() - keyFrameCamPos.get(i).getX())
* elapsedTime
/ totalTime;
camY =
keyFrameCamPos.get(i).getY()
+ (keyFrameCamPos.get(i + 1).getY() - keyFrameCamPos.get(i).getY())
* elapsedTime
/ totalTime;
camZ =
keyFrameCamPos.get(i).getZ()
+ (keyFrameCamPos.get(i + 1).getZ() - keyFrameCamPos.get(i).getZ())
* elapsedTime
/ totalTime;
fixX =
keyFrameFixPos.get(i).getX()
+ (keyFrameFixPos.get(i + 1).getX() - keyFrameFixPos.get(i).getX())
* elapsedTime
/ totalTime;
fixY =
keyFrameFixPos.get(i).getY()
+ (keyFrameFixPos.get(i + 1).getY() - keyFrameFixPos.get(i).getY())
* elapsedTime
/ totalTime;
fixZ =
keyFrameFixPos.get(i).getZ()
+ (keyFrameFixPos.get(i + 1).getZ() - keyFrameFixPos.get(i).getZ())
* elapsedTime
/ totalTime;
}
break;
}
}
}
public void gotoKey(int index) {
if ((index >= 0) && (index < keyFrameCamPos.size())) {
storedPositionIndex = index;
camX = keyFrameCamPos.get(index).getX();
camY = keyFrameCamPos.get(index).getY();
camZ = keyFrameCamPos.get(index).getZ();
fixX = keyFrameFixPos.get(index).getX();
fixY = keyFrameFixPos.get(index).getY();
fixZ = keyFrameFixPos.get(index).getZ();
}
}
public ArrayList<Integer> getCameraKeyPoints() {
return cameraKeyPoints;
}
@Override
public double getFixX() {
return this.fixX;
}
@Override
public double getFixY() {
return this.fixY;
}
@Override
public double getFixZ() {
return this.fixZ;
}
@Override
public double getCamX() {
return this.camX;
}
@Override
public double getCamY() {
return this.camY;
}
@Override
public double getCamZ() {
return this.camZ;
}
@Override
public void setFixX(double fx) {
this.fixX = fx;
}
@Override
public void setFixY(double fy) {
this.fixY = fy;
}
@Override
public void setFixZ(double fz) {
this.fixZ = fz;
}
@Override
public void setCamX(double cx) {
this.camX = cx;
}
@Override
public void setCamY(double cy) {
this.camY = cy;
}
@Override
public void setCamZ(double cz) {
this.camZ = cz;
}
@Override
public void setFixPosition(double fx, double fy, double fz) {
this.fixX = fx;
this.fixY = fy;
this.fixZ = fz;
}
@Override
public void setCameraPosition(double cx, double cy, double cz) {
this.camX = cx;
this.camY = cy;
this.camZ = cz;
}
private Vector3d v3d = new Vector3d();
private RigidBodyTransform t3d = new RigidBodyTransform();
private Vector3d rotVector = new Vector3d();
private AxisAngle4d rotAxisAngle4d = new AxisAngle4d();
public void doMouseDraggedLeft(double dx, double dy) {
// Rotate around fix point:
double delX0 = camX - fixX, delY0 = camY - fixY, delZ0 = camZ - fixZ;
v3d.set(delX0, delY0, delZ0);
// double offsetDistance = v3d.length();
t3d.setRotationYawAndZeroTranslation(-dx * rotate_factor);
t3d.transform(v3d);
if (!isDolly || (!isDollyX && !isDollyY)) {
camX = v3d.getX() + fixX;
camY = v3d.getY() + fixY;
}
delX0 = camX - fixX;
delY0 = camY - fixY;
delZ0 = camZ - fixZ;
// v3d.set(delX0, delY0, delZ0);
rotVector.cross(new Vector3d(0.0, 0.0, -1.0), v3d);
rotAxisAngle4d.set(rotVector, dy * rotate_factor / 4.0);
t3d.setRotationAndZeroTranslation(rotAxisAngle4d);
t3d.transform(v3d);
if ((v3d.getX() * delX0 > 0.0) && (v3d.getY() * delY0 > 0.0)) {
if (!isDolly || (!isDollyX && !isDollyY)) {
camX = v3d.getX() + fixX;
camY = v3d.getY() + fixY;
}
if (!isDolly || !isDollyZ) {
camZ = v3d.getZ() + fixZ;
/*
* double factor = elevate_factor * Math.abs(offsetDistance);
* //camZ-fixZ); if (factor < elevate_factor) factor =
* elevate_factor; //camZ = v3d.z + fixZ + dy * elevate_factor; camZ
* = v3d.z + fixZ + dy * factor;
*/
}
}
// transformChanged(currXform);
}
public void rotateAroundFix(double dx, double dy) {
double distanceFromCameraToFix =
Math.sqrt(Math.pow(camX - fixX, 2) + Math.pow(camY - fixY, 2) + Math.pow(camZ - fixZ, 2));
if (distanceFromCameraToFix > 1.0) {
dx /= distanceFromCameraToFix;
dy /= distanceFromCameraToFix;
}
double delX0 = camX - fixX, delY0 = camY - fixY, delZ0 = camZ - fixZ;
v3d.set(delX0, delY0, delZ0);
t3d.setRotationYawAndZeroTranslation(-dx * rotate_factor);
t3d.transform(v3d);
if (!isDolly || (!isDollyX && !isDollyY)) {
camX = v3d.getX() + fixX;
camY = v3d.getY() + fixY;
}
delX0 = camX - fixX;
delY0 = camY - fixY;
delZ0 = camZ - fixZ;
rotVector.cross(new Vector3d(0.0, 0.0, -1.0), v3d);
rotAxisAngle4d.set(rotVector, dy * rotate_factor / 4.0);
t3d.setRotationAndZeroTranslation(rotAxisAngle4d);
t3d.transform(v3d);
if ((v3d.getX() * delX0 > 0.0) && (v3d.getY() * delY0 > 0.0)) {
if (!isDolly || (!isDollyX && !isDollyY)) {
camX = v3d.getX() + fixX;
camY = v3d.getY() + fixY;
}
if (!isDolly || !isDollyZ) {
camZ = v3d.getZ() + fixZ;
}
}
}
public void doMouseDraggedRight(double dx, double dy) {
// Elevate up and down
double delX0 = camX - fixX, delY0 = camY - fixY, delZ0 = camZ - fixZ;
v3d.set(delX0, delY0, delZ0);
// double offsetDistance = v3d.length();
t3d.setRotationYawAndZeroTranslation(-dx * rotate_camera_factor);
t3d.transform(v3d);
if (!isTracking || (!isTrackingX && !isTrackingY)) {
fixX = camX - v3d.getX();
fixY = camY - v3d.getY();
}
delX0 = camX - fixX;
delY0 = camY - fixY;
delZ0 = camZ - fixZ;
// v3d.set(delX0, delY0, delZ0);
rotVector.set(-1.0, 0.0, 0.0);
rotVector.cross(new Vector3d(0.0, 0.0, -1.0), v3d);
rotAxisAngle4d.set(rotVector, dy * rotate_camera_factor / 4.0);
t3d.setRotationAndZeroTranslation(rotAxisAngle4d);
t3d.transform(v3d);
if ((v3d.getX() * delX0 > 0.0) && (v3d.getY() * delY0 > 0.0)) {
if (!isTracking || (!isTrackingX && !isTrackingY)) {
fixX = camX - v3d.getX();
fixY = camY - v3d.getY();
}
if (!isTracking || !isTrackingZ) {
fixZ = camZ - v3d.getZ();
/*
* double factor = elevate_camera_factor * offsetDistance;
* //Math.abs(camZ-fixZ); if (factor < elevate_camera_factor) factor
* = elevate_camera_factor; fixZ = camZ - v3d.z + dy factor; //fixZ
* = camZ - v3d.z + dy * elevate_factor;
*/
}
}
// transformChanged(currXform);
}
@Override
public void setFieldOfView(double fov) {
fieldOfView = fov;
if (fieldOfView < MIN_FIELD_OF_VIEW) fieldOfView = MIN_FIELD_OF_VIEW;
if (fieldOfView > MAX_FIELD_OF_VIEW) fieldOfView = MAX_FIELD_OF_VIEW;
}
public void doMouseDraggedMiddle(double dx, double dy) {
// Zooms in and out
if ((this.isMounted) && (viewportAdapter != null)) {
cameraMount.zoom(dy * 0.1);
} else {
Vector3d v3d = new Vector3d(camX - fixX, camY - fixY, camZ - fixZ);
Vector3d offsetVec = new Vector3d(v3d);
// offsetVec.normalize();
offsetVec.scale(dy * zoom_factor);
// if (offsetVec.length() < v3d.length())
// {
if (!isDolly || (!isDollyX && !isDollyY)) {
camX += offsetVec.getX();
camY += offsetVec.getY();
}
if (!isDolly || !isDollyZ) camZ += offsetVec.getZ();
// }
v3d.set(camX - fixX, camY - fixY, camZ - fixZ);
if (v3d.length() < MIN_CAMERA_POSITION_TO_FIX_DISTANCE) {
v3d.normalize();
v3d.scale(MIN_CAMERA_POSITION_TO_FIX_DISTANCE);
camX = v3d.getX() + fixX;
camY = v3d.getY() + fixY;
camZ = v3d.getZ() + fixZ;
}
}
// transformChanged(currXform);
}
private void moveCameraForward(double distance) {
double angleXY = Math.atan2(camY - fixY, camX - fixX);
double angleZ = Math.atan2(camZ - fixZ, Math.hypot(camY - fixY, camX - fixX));
double distXY = distance * Math.cos(angleZ);
camX += distXY * Math.cos(angleXY);
camY += distXY * Math.sin(angleXY);
camZ += distance * Math.sin(angleZ);
if (Math.sqrt(Math.pow(camX - fixX, 2) + Math.pow(camY - fixY, 2) + Math.pow(camY - fixY, 2))
< 1) {
fixX += distXY * Math.cos(angleXY);
fixY += distXY * Math.sin(angleXY);
fixZ += distance * Math.sin(angleZ);
}
// Vector3d v3d = new Vector3d(camX - fixX, camY - fixY, camZ - fixZ);
//
// Vector3d offsetVec = new Vector3d(v3d);
//
//// offsetVec.normalize();
// offsetVec.scale(distance * zoom_factor);
//
//// if (offsetVec.length() < v3d.length())
//// {
// if (!isDolly || (!isDollyX &&!isDollyY))
// {
// camX += offsetVec.x;
// camY += offsetVec.y;
// }
//
// if (!isDolly ||!isDollyZ)
// camZ += offsetVec.z;
}
public void pan(double dx, double dy) {
double distanceFromCameraToFix =
Math.sqrt(Math.pow(camX - fixX, 2) + Math.pow(camY - fixY, 2) + Math.pow(camZ - fixZ, 2));
dx *= distanceFromCameraToFix / viewportAdapter.getPhysicalWidth() * .00023;
dy *= distanceFromCameraToFix / viewportAdapter.getPhysicalHeight() * .00007;
double theta = Math.PI / 2 + Math.atan2((camZ - fixZ), Math.hypot(camX - fixX, camY - fixY));
if (!isTracking || !isTrackingZ) {
camZ += dy * Math.sin(theta);
fixZ += dy * Math.sin(theta);
}
double d = dy * Math.cos(theta);
theta = Math.atan2(camY - fixY, camX - fixX);
if (!isTracking || !isTrackingY) {
camY += d * Math.sin(theta);
fixY += d * Math.sin(theta);
}
if (!isTracking || !isTrackingX) {
camX += d * Math.cos(theta);
fixX += d * Math.cos(theta);
}
theta = Math.PI / 2 + Math.atan2(camY - fixY, camX - fixX);
if (!isTracking || !isTrackingY) {
camY -= dx * Math.sin(theta);
fixY -= dx * Math.sin(theta);
}
if (!isTracking || !isTrackingX) {
camX -= dx * Math.cos(theta);
fixX -= dx * Math.cos(theta);
}
}
public void translateFix(double dx, double dy, double dz) {
// double zTiltAngle = Math.PI / 2 + Math.atan2(camZ - fixZ, Math.hypot(camX - fixX, camY -
// fixY));
// double ky = dy * Math.sin(zTiltAngle);
// double kz = dz * Math.cos(zTiltAngle);
double yTiltAngle = Math.atan2(camY - fixY, camX - fixX);
if (!isTracking || !isTrackingZ) {
camZ += dz;
fixZ += dz;
}
if (!isTracking || !isTrackingY) {
camY += dy * Math.sin(yTiltAngle);
fixY += dy * Math.sin(yTiltAngle);
}
if (!isTracking || !isTrackingX) {
camX += dy * Math.cos(yTiltAngle);
fixX += dy * Math.cos(yTiltAngle);
}
double xTiltAngle = yTiltAngle + Math.PI / 2;
if (!isTracking || !isTrackingY) {
camY -= dx * Math.sin(xTiltAngle);
fixY -= dx * Math.sin(xTiltAngle);
}
if (!isTracking || !isTrackingX) {
camX -= dx * Math.cos(xTiltAngle);
fixX -= dx * Math.cos(xTiltAngle);
}
}
private void initTransition() {
camXSpeed = -(camX - storedCameraPositions.get(storedPositionIndex).getX()) / transitionTime;
camYSpeed = -(camY - storedCameraPositions.get(storedPositionIndex).getY()) / transitionTime;
camZSpeed = -(camZ - storedCameraPositions.get(storedPositionIndex).getZ()) / transitionTime;
fixXSpeed = -(fixX - storedFixPositions.get(storedPositionIndex).getX()) / transitionTime;
fixYSpeed = -(fixY - storedFixPositions.get(storedPositionIndex).getY()) / transitionTime;
fixZSpeed = -(fixZ - storedFixPositions.get(storedPositionIndex).getZ()) / transitionTime;
transitioning = true;
lastTransitionTime = System.currentTimeMillis();
}
@Override
public void toggleCameraKeyMode() {
setUseCameraKeyPoints(!useKeyCameraPoints());
}
public boolean getCameraKeyMode() {
return toggleCameraKeyPoints;
}
@Override
public void setUseCameraKeyPoints(boolean use) {
toggleCameraKeyPoints = use;
}
@Override
public boolean useKeyCameraPoints() {
return toggleCameraKeyPoints;
}
@Override
public boolean setCameraKeyPoint(int time) {
boolean added = false;
for (int i = 0; i < cameraKeyPoints.size(); i++) {
if (cameraKeyPoints.get(i) == time) {
addKeyFrame(removeKeyFrameByTime(time), time);
return false;
}
if (cameraKeyPoints.get(i) > time) {
cameraKeyPoints.add(time);
addKeyFrame(1, time);
return true;
}
}
if (!added) {
cameraKeyPoints.add(time);
addKeyFrame(time);
}
return true;
}
@Override
public void nextCameraKeyPoint(int time) {
// int closestLesserTime = Integer.MIN_VALUE;
// int index = -1;
// for (int i = 0; i < cameraKeyPoints.size(); i++)
// {
// if (cameraKeyPoints.get(i) < time && cameraKeyPoints.get(i) > closestLesserTime)
// {
// index = i;
// closestLesserTime = cameraKeyPoints.get(i);
// }
// }
// if (index != -1)
// {
// camera.initTransition(index);
// }
cameraKeyPointIndex++;
if (cameraKeyPointIndex >= cameraKeyPoints.size()) {
cameraKeyPointIndex = 0;
}
toggleCameraKeyPoints = false;
gotoKey(cameraKeyPointIndex);
}
@Override
public void previousCameraKeyPoint(int time) {
cameraKeyPointIndex--;
if (cameraKeyPointIndex < 0) {
cameraKeyPointIndex = cameraKeyPoints.size() - 1;
}
toggleCameraKeyPoints = false;
gotoKey(cameraKeyPointIndex);
}
@Override
public void removeCameraKeyPoint(int time) {
cameraKeyPoints.remove(cameraKeyPointIndex);
removeKeyFrameByIndex(cameraKeyPointIndex);
}
@Override
public double getTrackXVar() {
return cameraTrackAndDollyVariablesHolder.getTrackingX();
}
@Override
public double getTrackYVar() {
return cameraTrackAndDollyVariablesHolder.getTrackingY();
}
@Override
public double getTrackZVar() {
return cameraTrackAndDollyVariablesHolder.getTrackingZ();
}
@Override
public double getDollyXVar() {
return cameraTrackAndDollyVariablesHolder.getDollyX();
}
@Override
public double getDollyYVar() {
return cameraTrackAndDollyVariablesHolder.getDollyY();
}
@Override
public double getDollyZVar() {
return cameraTrackAndDollyVariablesHolder.getDollyZ();
}
public void nextStoredPosition() {
if (storedCameraPositions.size() > 0) {
storedPositionIndex++;
if (storedPositionIndex >= storedCameraPositions.size()) {
storedPositionIndex = 0;
}
initTransition();
}
}
public void previousStoredPosition() {
if (storedCameraPositions.size() > 0) {
storedPositionIndex--;
if (storedPositionIndex < 0) {
storedPositionIndex = storedCameraPositions.size() - 1;
}
initTransition();
}
}
public void storePosition() {
storedCameraPositions.add(new Point3d(getCamX(), getCamY(), getCamZ()));
storedFixPositions.add(new Point3d(getFixX(), getFixY(), getFixZ()));
}
@Override
public void reset() {}
private Matrix3d rotationMatrix = new Matrix3d();
private Vector3d positionOffset = new Vector3d();
private Vector3d zAxis = new Vector3d(), yAxis = new Vector3d(), xAxis = new Vector3d();
@Override
public void computeTransform(RigidBodyTransform currXform) {
update();
CameraMountInterface cameraMount = getCameraMount();
if (isMounted() && (cameraMount != null)) {
cameraMount.getTransformToCamera(currXform);
return;
}
positionOffset.set(getCamX(), getCamY(), getCamZ());
xAxis.set(getFixX(), getFixY(), getFixZ());
fixPointNode.translateTo(getFixX(), getFixY(), getFixZ());
xAxis.sub(positionOffset);
xAxis.normalize();
zAxis.set(0.0, 0.0, 1.0);
yAxis.cross(zAxis, xAxis);
zAxis.cross(xAxis, yAxis);
rotationMatrix.setColumn(0, xAxis);
rotationMatrix.setColumn(1, yAxis);
rotationMatrix.setColumn(2, zAxis);
currXform.setRotationAndZeroTranslation(rotationMatrix);
currXform.setTranslation(positionOffset);
currXform.normalizeRotationPart();
}
private boolean shouldAcceptDeviceInput() {
if (alreadyClosing
|| graphics3dAdapter.getContextManager().getCurrentViewport() != viewportAdapter)
return false;
if (jFrame != null && !jFrame.isActive()) return false;
return true;
}
@Override
public void keyPressed(Key key) {
if (shouldAcceptDeviceInput()) {
switch (key) {
case W:
forward = true;
break;
case S:
backward = true;
break;
case A:
left = true;
break;
case D:
right = true;
break;
case Q:
up = true;
break;
case Z:
down = true;
break;
default:
break;
}
}
}
@Override
public void keyReleased(Key key) {
if (shouldAcceptDeviceInput()) {
switch (key) {
case W:
forward = false;
break;
case S:
backward = false;
break;
case A:
left = false;
break;
case D:
right = false;
break;
case Q:
up = false;
break;
case Z:
down = false;
break;
case RIGHT:
nextStoredPosition();
break;
case LEFT:
previousStoredPosition();
break;
case K:
storePosition();
break;
default:
break;
}
}
}
@Override
public void selected(
Graphics3DNode graphics3dNode,
ModifierKeyInterface modifierKeyHolder,
Point3d location,
Point3d cameraLocation,
Quat4d cameraRotation) {
if (shouldAcceptDeviceInput()) {
if (modifierKeyHolder.isKeyPressed(Key.SHIFT)) {
if (!isTracking() || !isTrackingX()) setFixX(location.getX());
if (!isTracking() || !isTrackingY()) setFixY(location.getY());
if (!isTracking() || !isTrackingZ()) setFixZ(location.getZ());
}
}
}
@Override
public void mouseDragged(MouseButton mouseButton, double dx, double dy) {
if (shouldAcceptDeviceInput()) {
switch (mouseButton) {
case LEFT:
doMouseDraggedLeft(dx, dy);
break;
case RIGHT:
doMouseDraggedRight(dx, dy);
break;
case MIDDLE:
doMouseDraggedMiddle(dx, dy);
break;
case LEFTRIGHT:
pan(dx, dy);
break;
}
}
}
private double rotateGain = 0.15;
private double translateGain = 0.05;
@Override
public void mouseDragged(double dx, double dy, double dz, double drx, double dry, double drz) {
if (shouldAcceptDeviceInput()) {
// doMouseDraggedRight(drz, drx);
// doMouseDraggedMiddle(0.0, dz);
// moveCameraForward(dy);
rotateAroundFix(-drz * rotateGain, -drx * rotateGain);
translateFix(-dx * translateGain, -dy * translateGain, dz * translateGain);
}
}
@Override
public double getClipNear() {
if (isMounted) {
return cameraMount.getClipDistanceNear();
} else {
return clipDistanceNear;
}
}
@Override
public double getClipFar() {
if (isMounted) {
return cameraMount.getClipDistanceFar();
} else {
return clipDistanceFar;
}
}
@Override
public double getHorizontalFieldOfViewInRadians() {
if (isMounted) {
return cameraMount.getFieldOfView();
} else {
return fieldOfView;
}
}
@Override
public void setClipDistanceNear(double near) {
clipDistanceNear = near;
}
@Override
public void setClipDistanceFar(double far) {
clipDistanceFar = far;
}
@Override
public void copyPositionTrackingDollyConfiguration(TrackingDollyCameraController otherCamera) {
setTracking(
otherCamera.isTracking(),
otherCamera.isTrackingX(),
otherCamera.isTrackingY(),
otherCamera.isTrackingZ());
setDolly(
otherCamera.isDolly(),
otherCamera.isDollyX(),
otherCamera.isDollyY(),
otherCamera.isDollyZ());
setCameraPosition(otherCamera.getCamX(), otherCamera.getCamY(), otherCamera.getCamZ());
setFixPosition(otherCamera.getFixX(), otherCamera.getFixY(), otherCamera.getFixZ());
setDollyOffsets(
otherCamera.getDollyXOffset(),
otherCamera.getDollyYOffset(),
otherCamera.getDollyZOffset());
setTrackingOffsets(
otherCamera.getTrackingXOffset(),
otherCamera.getTrackingYOffset(),
otherCamera.getTrackingZOffset());
if (otherCamera instanceof ClassicCameraController) {
ClassicCameraController classicOtherCamera = (ClassicCameraController) otherCamera;
keyFrameCamPos = classicOtherCamera.keyFrameCamPos;
keyFrameFixPos = classicOtherCamera.keyFrameFixPos;
keyFrameTimes = classicOtherCamera.keyFrameTimes;
toggleCameraKeyPoints = classicOtherCamera.toggleCameraKeyPoints;
cameraKeyPointIndex = classicOtherCamera.cameraKeyPointIndex;
cameraKeyPoints = classicOtherCamera.cameraKeyPoints;
System.out.println("Copying camera keys");
}
}
private boolean alreadyClosing = false;
@Override
public void closeAndDispose() {
if (alreadyClosing) return;
alreadyClosing = true;
this.cameraMount = null;
this.viewportAdapter = null;
if (cameraTrackAndDollyVariablesHolder != null) {
cameraTrackAndDollyVariablesHolder.closeAndDispose();
cameraTrackAndDollyVariablesHolder = null;
}
graphics3dAdapter = null;
}
public Graphics3DNode getFixPointNode() {
return fixPointNode;
}
}
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();
}