public class DRCDebrisEnvironment implements CommonAvatarEnvironmentInterface { private final ReferenceFrame constructionWorldFrame = ReferenceFrame.constructAWorldFrame("constructionFrame"); private final CombinedTerrainObject3D combinedTerrainObject; private final ArrayList<ExternalForcePoint> contactPoints = new ArrayList<ExternalForcePoint>(); private final List<ContactableSelectableBoxRobot> debrisRobots = new ArrayList<ContactableSelectableBoxRobot>(); private final String debrisName = "Debris"; private int id = 0; private final double debrisDepth = 0.0508; private final double debrisWidth = 0.1016; private final double debrisLength = 0.9144; private final double debrisMass = 1.0; private final PoseReferenceFrame robotInitialPoseReferenceFrame; private final double forceVectorScale = 1.0 / 50.0; public DRCDebrisEnvironment() { this(new Vector3d(0.0, 0.0, 0.0), 0.0); } 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); } public void addStandingDebris( double xRelativeToRobot, double yRelativeToRobot, double debrisYaw) { Point3d tempPosition = new Point3d(xRelativeToRobot, yRelativeToRobot, debrisLength / 2.0); FramePose debrisPose = generateDebrisPose(tempPosition, debrisYaw, 0.0, 0.0); debrisRobots.add(createDebrisRobot(debrisPose)); } 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 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()); } private FramePose generateDebrisPose( Point3d positionWithRespectToRobot, double debrisYaw, double debrisPitch, double debrisRoll) { FramePose debrisPose = new FramePose(robotInitialPoseReferenceFrame); Quat4d orientation = new Quat4d(); RotationTools.convertYawPitchRollToQuaternion(debrisYaw, debrisPitch, debrisRoll, orientation); debrisPose.setPose(positionWithRespectToRobot, orientation); debrisPose.changeFrame(constructionWorldFrame); return debrisPose; } 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 createDebrisContactController() { for (int i = 0; i < debrisRobots.size(); i++) { ContactableSelectableBoxRobot debrisRobot = debrisRobots.get(i); GroundContactModel groundContactModel = createGroundContactModel(debrisRobot, combinedTerrainObject); debrisRobot.createAvailableContactPoints(1, 20, forceVectorScale, true); debrisRobot.setGroundContactModel(groundContactModel); } } private ContactableSelectableBoxRobot createDebrisRobot(FramePose debrisPose) { debrisPose.checkReferenceFrameMatch(constructionWorldFrame); ContactableSelectableBoxRobot debris = ContactableSelectableBoxRobot.createContactable2By4Robot( debrisName + String.valueOf(id++), debrisDepth, debrisWidth, debrisLength, debrisMass); debris.setPosition(debrisPose.getX(), debrisPose.getY(), debrisPose.getZ()); debris.setYawPitchRoll(debrisPose.getYaw(), debrisPose.getPitch(), debrisPose.getRoll()); return debris; } private GroundContactModel createGroundContactModel(Robot robot, GroundProfile3D groundProfile) { double kXY = 5000.0; double bXY = 100.0; double kZ = 1000.0; double bZ = 100.0; double alphaStick = 0.7; double alphaSlip = 0.5; GroundContactModel groundContactModel = new LinearStickSlipGroundContactModel( robot, kXY, bXY, kZ, bZ, alphaSlip, alphaStick, robot.getRobotsYoVariableRegistry()); groundContactModel.setGroundProfile3D(groundProfile); return groundContactModel; } @Override public TerrainObject3D getTerrainObject3D() { return combinedTerrainObject; } @Override public List<ContactableSelectableBoxRobot> getEnvironmentRobots() { return debrisRobots; } @Override public void createAndSetContactControllerToARobot() { // add contact controller to any robot so it gets called ContactController contactController = new ContactController("DebrisContactController"); contactController.setContactParameters(1000.0, 100.0, 0.5, 0.3); contactController.addContactPoints(contactPoints); contactController.addContactables(debrisRobots); debrisRobots.get(0).setController(contactController); } @Override public void addContactPoints(List<? extends ExternalForcePoint> externalForcePoints) { this.contactPoints.addAll(externalForcePoints); } @Override public void addSelectableListenerToSelectables(SelectableObjectListener selectedListener) {} public double getDebrisDepth() { return debrisDepth; } public double getDebrisWidth() { return debrisWidth; } public double getDebrisLength() { return debrisLength; } }