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); }
/** 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); }
/** 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); }
public double getBodyPitchVel() { return bodyJoint2.getQD(); }
// ************************* Part 2 public double getBodyPitch() { return bodyJoint2.getQ(); }