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 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 rotate(double angle, Axis axis) {
TransformTools.rotate(transform, angle, axis);
}