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); }
robot.getRobotsYoVariableRegistry()); externalForcePoints.add(efp); jointToAddExternalForcePoints.addExternalForcePoint(efp);
robot.getRobotsYoVariableRegistry()); externalForcePoints.add(efp); jointToAddExternalForcePoints.addExternalForcePoint(efp);
robot.getRobotsYoVariableRegistry()); externalForcePoints.add(efp); jointToAddExternalForcePoints.addExternalForcePoint(efp);
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()); }
leftAnkleJoint.addExternalForcePoint(leftFootExternalForcePoint); rightAnkleJoint.addExternalForcePoint(rightFootExternalForcePoint);
handControlFramePositionInWorld = new Point3d(); wristJoint.addExternalForcePoint(efpWrist); wristJoint.addExternalForcePoint(efpHandControlFrame);
handControlFramePositionInWorld = new Point3D(); wristJoint.addExternalForcePoint(efpWrist); wristJoint.addExternalForcePoint(efpHandControlFrame);
handControlFramePositionInWorld = new Point3d(); wristJoint.addExternalForcePoint(efpWrist); wristJoint.addExternalForcePoint(efpHandControlFrame);