/** 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); }