public static void addCube(
double cubeX, double cubeY, double cubeSize, Color color, SimulationConstructionSet scs) {
final double CUBE_HEIGHT = 0.001;
// Make cubes a tiny bit smaller so they don't blend together.
cubeSize *= 0.9;
Graphics3DObject linkGraphics = new Graphics3DObject();
linkGraphics.translate(new Vector3d(cubeX, cubeY, CUBE_HEIGHT));
linkGraphics.addCube(
cubeSize, cubeSize, cubeSize, new YoAppearanceRGBColor(new Color3f(color), 0.0));
scs.addStaticLinkGraphics(linkGraphics);
}
/** *********************** SCS ROBOT - Links ******************************* */
private void createAndAttachBodyLink(LinkNames linkName) {
Link link = new Link(LinkNames.BODY_LINK.getName());
Matrix3d inertiaBody = createInertiaMatrixBox(linkName);
link.setMomentOfInertia(inertiaBody);
link.setMass(RobotParameters.MASSES.get(LinkNames.BODY_LINK));
link.setComOffset(comOffsetBody);
Graphics3DObject linkGraphics = new Graphics3DObject();
linkGraphics.addCube(cubeX, cubeY, cubeZ, RobotParameters.APPEARANCE.get(LinkNames.BODY_LINK));
link.setLinkGraphics(linkGraphics);
// link.addEllipsoidFromMassProperties(YoAppearance.Green());
link.addCoordinateSystemToCOM(0.7);
bodyJointSCS.setLink(link);
}
private void createAndAttachFootLink(
LinkNames linkName, JointNames jointName, RobotSide robotSide) {
Link link = new Link(LinkNames.FOOT_LINK.getName());
Matrix3d inertiaFoot = createInertiaMatrixBox(linkName);
link.setMomentOfInertia(inertiaFoot);
link.setMass(RobotParameters.MASSES.get(LinkNames.FOOT_LINK));
link.setComOffset(comOffsetFoot);
Graphics3DObject linkGraphics = new Graphics3DObject();
linkGraphics.translate(footOffsetX, 0.0, 0.0);
linkGraphics.addCube(footX, footY, -footZ, RobotParameters.APPEARANCE.get(LinkNames.FOOT_LINK));
link.setLinkGraphics(linkGraphics);
// link.addEllipsoidFromMassProperties(YoAppearance.Green());
link.addCoordinateSystemToCOM(0.3);
idToSCSLegJointMap.get(allLegJoints.get(robotSide).get(jointName)).setLink(link);
}
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;
}