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;
}
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;
}
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());
}
public DRCDoorEnvironment() {
combinedTerrainObject = new CombinedTerrainObject3D(getClass().getSimpleName());
combinedTerrainObject.addTerrainObject(setUpGround("Ground"));
ContactableDoorRobot door = new ContactableDoorRobot("doorRobot", new Point3d(3.0, 0.0, 0.0));
doorRobots.add(door);
door.createAvailableContactPoints(0, 15, 15, 0.02, true);
}
public DRCDebrisEnvironment(Tuple3d robotInitialPosition, double robotInitialYaw) {
combinedTerrainObject = new CombinedTerrainObject3D(getClass().getSimpleName());
combinedTerrainObject.addTerrainObject(setUpGround("Ground"));
Quat4d robotInitialOrientation = new Quat4d();
RotationTools.convertYawPitchRollToQuaternion(
robotInitialYaw, 0.0, 0.0, robotInitialOrientation);
Point3d robotPosition = new Point3d(robotInitialPosition);
FramePose robotInitialPose =
new FramePose(constructionWorldFrame, robotPosition, robotInitialOrientation);
this.robotInitialPoseReferenceFrame =
new PoseReferenceFrame("robotInitialPoseReferenceFrame", robotInitialPose);
}
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);
}
}
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());
}
private void addGround() {
combinedTerrainObject.addBox(-10.0, -10.0, 10.0, 10.0, -0.05, 0.0);
}