/** 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 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);
}
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;
}
/** *********************** 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);
}
/** This method returns a brick link instance. */
private Link base(String name, AppearanceDefinition appearance) {
Link ret = new Link(name);
ret.setMass(M1);
ret.setMomentOfInertia(Ixx1, Iyy1, Izz1);
ret.setComOffset(0.0, 0.0, 0.0);
Graphics3DObject linkGraphics = new Graphics3DObject();
linkGraphics.translate(0.0, 0.0, -B1);
// linkGraphics.addCube((float)BASE_L, (float)BASE_W, (float)BASE_H, appearance);
// linkGraphics.addCone((float)BASE_L,(float)BASE_W);
linkGraphics.addPyramidCube(BASE_L, BASE_W, BASE_H, BASE_H, appearance);
ret.setLinkGraphics(linkGraphics);
return ret;
}
private void createAndAttachCylinderLink(
LinkNames linkName, JointNames jointName, RobotSide robotSide) {
Link link = new Link(linkName.getName());
Matrix3d inertiaCylinder = createInertiaMatrixCylinder(linkName);
link.setMomentOfInertia(inertiaCylinder);
link.setMass(RobotParameters.MASSES.get(linkName));
link.setComOffset(new Vector3d(0.0, 0.0, -RobotParameters.LENGTHS.get(linkName) / 2.0));
Graphics3DObject linkGraphics = new Graphics3DObject();
linkGraphics.addCylinder(
-RobotParameters.LENGTHS.get(linkName),
RobotParameters.RADII.get(linkName),
RobotParameters.APPEARANCE.get(linkName));
link.setLinkGraphics(linkGraphics);
// link.addEllipsoidFromMassProperties(YoAppearance.Green());
link.addCoordinateSystemToCOM(0.3);
idToSCSLegJointMap.get(allLegJoints.get(robotSide).get(jointName)).setLink(link);
}
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;
}
public void notifySelectedListeners(
ModifierKeyInterface modifierKeys,
Point3d location,
Point3d cameraPosition,
Quat4d cameraRotation) {
for (SelectedListener selectedListener : selectedListeners) {
selectedListener.selected(this, modifierKeys, location, cameraPosition, cameraRotation);
}
graphicsObject.notifySelectedListeners(
this, modifierKeys, location, cameraPosition, cameraRotation);
}
public SimpleTableTerrainObject(
double xStart, double yStart, double xEnd, double yEnd, double zStart, double zEnd) {
this.TABLE_LENGTH = Math.abs(xStart - xEnd);
this.TABLE_WIDTH = Math.abs(yStart - yEnd);
this.TABLE_THICKNESS = Math.abs(zStart - zEnd);
double xMin = Math.min(xStart, xEnd);
double xMax = Math.max(xStart, xEnd);
double yMin = Math.min(yStart, yEnd);
double yMax = Math.max(yStart, yEnd);
double zMin = Math.min(zStart, zEnd);
double zMax = Math.max(zStart, zEnd);
Point3d minPoint = new Point3d(xMin, yMin, zMin);
Point3d maxPoint = new Point3d(xMax, yMax, zMax);
boundingBox = new BoundingBox3d(minPoint, maxPoint);
linkGraphics = new Graphics3DObject();
linkGraphics.translate(
(xStart + xEnd) / 2.0, (yStart + yEnd) / 2.0, zMin + TABLE_THICKNESS / 2);
linkGraphics.scale(new Vector3d(TABLE_LENGTH, TABLE_WIDTH, TABLE_THICKNESS));
linkGraphics.addModelFile("models/plasticTableTop.obj");
if (TABLE_LENGTH < TABLE_WIDTH) linkGraphics.rotate(Math.PI / 2, Axis.Z);
linkGraphics.scale(new Vector3d(1, 1, boundingBox.getZMax() / TABLE_THICKNESS));
linkGraphics.addModelFile("models/FoldingTableLegs.obj");
}
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 void addCoordinateFrame() {
Graphics3DObject coordinateFrameObject = new Graphics3DObject();
coordinateFrameObject.addCoordinateSystem(1.0);
addChild(new Graphics3DNode("CoordinateFrameNode", coordinateFrameObject));
}
/** 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);
}
