/** 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);
}
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;
}
public FallingSphereRobot(String name, boolean useImpulseGroundModel) {
super(name);
this.setGravity(0.0, 0.0, -G);
// Base:
floatingJoint = new FloatingJoint("base", new Vector3d(0.0, 0.0, 0.0), this);
Link link1 = ball();
floatingJoint.setLink(link1);
this.addRootJoint(floatingJoint);
YoGraphicsListRegistry yoGraphicsListRegistry = new YoGraphicsListRegistry();
// Ground contact points:
int N = 20; // 8;
for (int i = 0; i < N; i++) {
double latitude = -Math.PI / 2.0 + (i * Math.PI) / N;
int nForThisLatitude = (int) ((Math.cos(latitude) * N) + 0.5);
for (int j = 0; j < nForThisLatitude; j++) {
double longitude = (j * 2.0 * Math.PI) / nForThisLatitude;
double z = R * Math.sin(latitude);
double x = R * Math.cos(latitude) * Math.cos(longitude);
double y = R * Math.cos(latitude) * Math.sin(longitude);
// System.out.println("x,y,z: " + x + ", " + y + ", " + z);
String gcName = "gc" + i + "_" + j;
GroundContactPoint gc = new GroundContactPoint(gcName, new Vector3d(x, y, z), this);
floatingJoint.addGroundContactPoint(gc);
YoGraphicPosition dynamicGraphicPosition =
new YoGraphicPosition(
gcName + "Position", gc.getYoPosition(), 0.01, YoAppearance.Red());
yoGraphicsListRegistry.registerYoGraphic("FallingSphereGCPoints", dynamicGraphicPosition);
if (useImpulseGroundModel) {
YoGraphicVector dynamicGraphicVector =
new YoGraphicVector(
gcName + "Force",
gc.getYoPosition(),
gc.getYoImpulse(),
10.0,
YoAppearance.Pink());
yoGraphicsListRegistry.registerYoGraphic("FallingSphereForces", dynamicGraphicVector);
} else {
YoGraphicVector dynamicGraphicVector =
new YoGraphicVector(
gcName + "Force", gc.getYoPosition(), gc.getYoForce(), 1.0 / 50.0);
yoGraphicsListRegistry.registerYoGraphic("FallingSphereForces", dynamicGraphicVector);
}
}
}
GroundContactModel groundContactModel;
if (useImpulseGroundModel) {
groundContactModel = new CollisionGroundContactModel(this, EPSILON, MU, registry);
} else {
double kXY = 1000.0; // 1422.0;
double bXY = 100.0; // 150.6;
double kZ = 20.0; // 50.0;
double bZ = 50.0; // 1000.0;
groundContactModel = new LinearGroundContactModel(this, kXY, bXY, kZ, bZ, registry);
}
double amplitude = 0.1;
double frequency = 0.3;
double offset = 0.5;
GroundProfile3D groundProfile = new RollingGroundProfile(amplitude, frequency, offset);
groundContactModel.setGroundProfile3D(groundProfile);
this.setGroundContactModel(groundContactModel);
initRobot();
this.getRobotsYoVariableRegistry().addChild(registry);
this.addDynamicGraphicObjectsListRegistry(yoGraphicsListRegistry);
}
public FallingBrickRobot() {
super("FallingBrick");
this.setGravity(0.0, 0.0, -G);
// create the brick as a floating joint
floatingJoint = new FloatingJoint("base", new Vector3d(0.0, 0.0, 0.0), this);
Link link1 = base("base", YoAppearance.Red());
floatingJoint.setLink(link1);
this.addRootJoint(floatingJoint);
// add ground contact points to the brick
GroundContactPoint gc1 =
new GroundContactPoint("gc1", new Vector3d(BASE_L / 2.0, BASE_W / 2.0, BASE_H / 2.0), this);
floatingJoint.addGroundContactPoint(gc1);
GroundContactPoint gc2 =
new GroundContactPoint(
"gc2", new Vector3d(BASE_L / 2.0, -BASE_W / 2.0, BASE_H / 2.0), this);
floatingJoint.addGroundContactPoint(gc2);
GroundContactPoint gc3 =
new GroundContactPoint(
"gc3", new Vector3d(-BASE_L / 2.0, BASE_W / 2.0, BASE_H / 2.0), this);
floatingJoint.addGroundContactPoint(gc3);
GroundContactPoint gc4 =
new GroundContactPoint(
"gc4", new Vector3d(-BASE_L / 2.0, -BASE_W / 2.0, BASE_H / 2.0), this);
floatingJoint.addGroundContactPoint(gc4);
GroundContactPoint gc5 =
new GroundContactPoint(
"gc5", new Vector3d(BASE_L / 2.0, BASE_W / 2.0, -BASE_H / 2.0), this);
floatingJoint.addGroundContactPoint(gc5);
GroundContactPoint gc6 =
new GroundContactPoint(
"gc6", new Vector3d(BASE_L / 2.0, -BASE_W / 2.0, -BASE_H / 2.0), this);
floatingJoint.addGroundContactPoint(gc6);
GroundContactPoint gc7 =
new GroundContactPoint(
"gc7", new Vector3d(-BASE_L / 2.0, BASE_W / 2.0, -BASE_H / 2.0), this);
floatingJoint.addGroundContactPoint(gc7);
GroundContactPoint gc8 =
new GroundContactPoint(
"gc8", new Vector3d(-BASE_L / 2.0, -BASE_W / 2.0, -BASE_H / 2.0), this);
floatingJoint.addGroundContactPoint(gc8);
GroundContactPoint gc9 =
new GroundContactPoint("gc9", new Vector3d(0.0, 0.0, BASE_H / 2.0 + BASE_H), this);
floatingJoint.addGroundContactPoint(gc9);
GroundContactPoint gc10 =
new GroundContactPoint("gc10", new Vector3d(0.0, 0.0, -BASE_H / 2.0 - BASE_H), this);
floatingJoint.addGroundContactPoint(gc10);
this.setController(this); // tells the simulator to call the local doControl() method
// instantiate ground contact model
GroundContactModel groundModel =
new LinearGroundContactModel(
this, 1422, 150.6, 50.0, 1000.0, this.getRobotsYoVariableRegistry());
// GroundContactModel groundModel = new CollisionGroundContactModel(this, 0.5, 0.7);
GroundProfile3D profile = new WavyGroundProfile();
groundModel.setGroundProfile3D(profile);
this.setGroundContactModel(groundModel);
initRobot();
initControl();
}
/** 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);
}
