/** 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 DoublePendulum() {
super("DoublePendulum");
j1 = new PinJoint("j1", new Vector3d(0, 0, 2), this, Axis.X);
Link l1 = new Link("l1");
l1.setComOffset(0, 0, 0.5);
l1.setMassAndRadiiOfGyration(1.0, 0.05, 0.05, 0.3);
l1.addEllipsoidFromMassProperties(YoAppearance.Pink());
j1.setLink(l1);
j2 = new PinJoint("j2", new Vector3d(0.0, 0.0, 1.0), this, Axis.X);
Link l2 = new Link("l2");
l2.setComOffset(0, 0, 0.5);
l2.setMassAndRadiiOfGyration(1.0, 0.05, 0.05, 0.3);
l2.addEllipsoidFromMassProperties(YoAppearance.Purple());
j2.setLink(l2);
j1.addJoint(j2);
addRootJoint(j1);
}