private void addForcePoint() { externalForcePoint = new ExternalForcePoint("handForcePoint", scsRobotArm.getLink("hand").getComOffset(), scsRobotArm); scsRobotArm.getLink("hand").getParentJoint().addExternalForcePoint(externalForcePoint); }
private void addForcePoint() { externalForcePoint1 = new ExternalForcePoint("hand1ForcePoint", scsRobotArm.getLink("hand1").getComOffset(), scsRobotArm); externalForcePoint2 = new ExternalForcePoint("hand2ForcePoint", scsRobotArm.getLink("hand2").getComOffset(), scsRobotArm); scsRobotArm.getLink("hand1").getParentJoint().addExternalForcePoint(externalForcePoint1); scsRobotArm.getLink("hand2").getParentJoint().addExternalForcePoint(externalForcePoint2); }
private void addForcePoint() { externalForcePoint = new ExternalForcePoint("handForcePoint", scsRobotArm.getLink("hand").getComOffset(), scsRobotArm); scsRobotArm.getLink("hand").getParentJoint().addExternalForcePoint(externalForcePoint); }
private void addForcePoint() { externalForcePoint1 = new ExternalForcePoint("hand1ForcePoint", scsRobotArm.getLink("hand1").getComOffset(), scsRobotArm); externalForcePoint2 = new ExternalForcePoint("hand2ForcePoint", scsRobotArm.getLink("hand2").getComOffset(), scsRobotArm); scsRobotArm.getLink("hand1").getParentJoint().addExternalForcePoint(externalForcePoint1); scsRobotArm.getLink("hand2").getParentJoint().addExternalForcePoint(externalForcePoint2); }
public VirtualHoist(Joint jointToAttachHoist, Robot robot, ArrayList<Vector3D> hoistPointPositions, double updateDT) { this.updateDT = updateDT; for (int i = 0; i < hoistPointPositions.size(); i++) { Vector3D hoistPointPosition = hoistPointPositions.get(i); ExternalForcePoint externalForcePoint = new ExternalForcePoint("ef_hoist" + i, hoistPointPosition, robot.getRobotsYoVariableRegistry()); externalForcePoints.add(externalForcePoint); jointToAttachHoist.addExternalForcePoint(externalForcePoint); cableLengths.add(new YoDouble("hoistCableLength" + i, registry)); cableForceMagnitudes.add(new YoDouble("hoistCableForceMagnitude" + i, registry)); } physicalCableLength.set(0.5); // Initial gains and teepee location: hoistStiffness.set(5000.0); hoistDamping.set(1000.0); hoistUpDownSpeed.set(0.08); turnHoistOff(); hoistUp.set(false); hoistDown.set(false); q_x = (YoDouble) robot.getVariable("q_x"); q_y = (YoDouble) robot.getVariable("q_y"); q_z = (YoDouble) robot.getVariable("q_z"); teepeeLocation.set(0.0, 0.0, 1.25); }
ExternalForcePoint efp = new ExternalForcePoint("efp_" + linkName + "_" + String.valueOf(i) + "_", efp_offsetFromRootJoint.get(i), robot.getRobotsYoVariableRegistry()); externalForcePoints.add(efp);
ExternalForcePoint efp = new ExternalForcePoint("efp_" + linkName + "_" + String.valueOf(i) + "_", efp_offsetFromRootJoint.get(i), robot.getRobotsYoVariableRegistry()); externalForcePoints.add(efp);
ExternalForcePoint efp = new ExternalForcePoint("efp_" + linkName + "_" + String.valueOf(i) + "_", efp_offsetFromRootJoint.get(i), robot.getRobotsYoVariableRegistry()); externalForcePoints.add(efp);
rootJointExternalForcePoint = new ExternalForcePoint("rootJointExternalForce", robot); rootJoint.addExternalForcePoint(rootJointExternalForcePoint); rightAnkleJoint.addIMUMount(rightIMUMount); ExternalForcePoint leftFootExternalForcePoint = new ExternalForcePoint("leftFootExternalForcePoint", robot); leftAnkleJoint.addExternalForcePoint(leftFootExternalForcePoint); ExternalForcePoint rightFootExternalForcePoint = new ExternalForcePoint("rightFootExternalForcePoint", robot); rightAnkleJoint.addExternalForcePoint(rightFootExternalForcePoint);
public PushRobotController(FloatingRootJointRobot pushableRobot, String jointNameToApplyForce, Vector3d forcePointOffset) { yoTime = pushableRobot.getYoTime(); registry = new YoVariableRegistry(jointNameToApplyForce + "_" + getClass().getSimpleName()); forcePoint = new ExternalForcePoint(jointNameToApplyForce + "_externalForcePoint", forcePointOffset, pushableRobot); pushDuration = new DoubleYoVariable(jointNameToApplyForce + "_pushDuration", registry); pushForceMagnitude = new DoubleYoVariable(jointNameToApplyForce + "_pushMagnitude", registry); pushDirection = new YoFrameVector(jointNameToApplyForce + "_pushDirection", worldFrame, registry); pushForce = new YoFrameVector(jointNameToApplyForce + "_pushForce", worldFrame, registry); pushTimeSwitch = new DoubleYoVariable(jointNameToApplyForce + "_pushTimeSwitch", registry); pushNumber = new IntegerYoVariable(jointNameToApplyForce + "_pushNumber", registry); isBeingPushed = new BooleanYoVariable(jointNameToApplyForce + "_isBeingPushed", registry); pushDelay = new DoubleYoVariable(jointNameToApplyForce + "_pushDelay", registry); pushableRobot.getJoint(jointNameToApplyForce).addExternalForcePoint(forcePoint); pushableRobot.setController(this); pushTimeSwitch.set(Double.NEGATIVE_INFINITY); pushForceMagnitude.set(0.0); forceVisualizer = new YoGraphicVector(jointNameToApplyForce + "_pushForce", forcePoint.getYoPosition(), forcePoint.getYoForce(), 0.005, YoAppearance.DarkBlue()); }
public PushRobotController(FloatingRootJointRobot pushableRobot, String jointNameToApplyForce, Vector3D forcePointOffset, double visualScale) { yoTime = pushableRobot.getYoTime(); registry = new YoVariableRegistry(jointNameToApplyForce + "_" + getClass().getSimpleName()); forcePoint = new ExternalForcePoint(jointNameToApplyForce + "_externalForcePoint", forcePointOffset, pushableRobot); pushDuration = new YoDouble(jointNameToApplyForce + "_pushDuration", registry); pushForceMagnitude = new YoDouble(jointNameToApplyForce + "_pushMagnitude", registry); pushDirection = new YoFrameVector3D(jointNameToApplyForce + "_pushDirection", worldFrame, registry); pushForce = new YoFrameVector3D(jointNameToApplyForce + "_pushForce", worldFrame, registry); pushTimeSwitch = new YoDouble(jointNameToApplyForce + "_pushTimeSwitch", registry); pushNumber = new YoInteger(jointNameToApplyForce + "_pushNumber", registry); isBeingPushed = new YoBoolean(jointNameToApplyForce + "_isBeingPushed", registry); pushDelay = new YoDouble(jointNameToApplyForce + "_pushDelay", registry); pushableRobot.getJoint(jointNameToApplyForce).addExternalForcePoint(forcePoint); pushableRobot.setController(this); pushTimeSwitch.set(Double.NEGATIVE_INFINITY); pushForceMagnitude.set(0.0); forceVisualizer = new YoGraphicVector(jointNameToApplyForce + "_pushForce", forcePoint.getYoPosition(), forcePoint.getYoForce(), visualScale, YoAppearance.DarkBlue()); }
ExternalForcePoint externalForcePoint = new ExternalForcePoint("rootExternalForcePoint", externalForcePointOffset, robot.getRobotsYoVariableRegistry()); rootJoint.addExternalForcePoint(externalForcePoint); externalForcePoint.setForce(random.nextDouble(), random.nextDouble(), random.nextDouble());
ExternalForcePoint externalForcePoint = new ExternalForcePoint("efp", externalForcePointOffset, robot.getRobotsYoVariableRegistry()); root1.addExternalForcePoint(externalForcePoint); Vector3D force = new Vector3D(random.nextDouble(), random.nextDouble(), random.nextDouble());
efpWrist = new ExternalForcePoint("wrist", sdfRobot); efpHandControlFrame = new ExternalForcePoint("tooltip", wristToHandControlFrame, sdfRobot); handControlFramePositionInWorld = new Point3d();
efpWrist = new ExternalForcePoint("wrist", sdfRobot); efpHandControlFrame = new ExternalForcePoint("tooltip", wristToHandControlFrame, sdfRobot); handControlFramePositionInWorld = new Point3D();
efpWrist = new ExternalForcePoint("wrist", sdfRobot); efpHandControlFrame = new ExternalForcePoint("tooltip", wristToHandControlFrame, sdfRobot); handControlFramePositionInWorld = new Point3d();
staticLinkGraphics = staticLink.getLinkGraphics(); graphics3DNodeType = Graphics3DNodeType.GROUND; simpleExternalForcePoint = new ExternalForcePoint("simpleExternalForcePoint", dummyRegistry); yoGraphic = new YoGraphicVector("simpleDynamicGraphicObject", simpleExternalForcePoint.getYoPosition(), simpleExternalForcePoint.getYoForce(), 1.0 / 50.0);
bodyLink.setMassAndRadiiOfGyration(10.0, 0.1, 0.2, 0.3); ExternalForcePoint externalForcePoint = new ExternalForcePoint("ef_rootJoint", this.getRobotsYoVariableRegistry()); rootJoint.addExternalForcePoint(externalForcePoint);
floatingJoint.setLink(link); ExternalForcePoint externalForcePoint = new ExternalForcePoint("externalForcePoint", new Vector3D(), robot); floatingJoint.addExternalForcePoint(externalForcePoint);