private YoVariable<?>[] setUpVariableList(String variableOrderedList) { StringTokenizer tokenizer = new StringTokenizer(variableOrderedList, ","); YoVariable<?>[] vars = new YoVariable[tokenizer.countTokens()]; int index = 0; while (tokenizer.hasMoreTokens()) { String varName = tokenizer.nextToken(); vars[index] = terminator.getVariable(varName); index++; } return vars; }
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); }
private void assertJointWrenchSensorConsistency(Robot robot, JointWrenchSensor jointWrenchSensor) { Tuple3DBasics jointForce = new Vector3D(); Tuple3DBasics jointTorque = new Vector3D(); jointWrenchSensor.getJointForce(jointForce); jointWrenchSensor.getJointTorque(jointTorque); String name = jointWrenchSensor.getName(); YoDouble forceX = (YoDouble) robot.getVariable(name + "_fX"); YoDouble forceY = (YoDouble) robot.getVariable(name + "_fY"); YoDouble forceZ = (YoDouble) robot.getVariable(name + "_fZ"); YoDouble torqueX = (YoDouble) robot.getVariable(name + "_tX"); YoDouble torqueY = (YoDouble) robot.getVariable(name + "_tY"); YoDouble torqueZ = (YoDouble) robot.getVariable(name + "_tZ"); assertEquals(jointForce.getX(), forceX.getDoubleValue(), 1e-7); assertEquals(jointForce.getY(), forceY.getDoubleValue(), 1e-7); assertEquals(jointForce.getZ(), forceZ.getDoubleValue(), 1e-7); assertEquals(jointTorque.getX(), torqueX.getDoubleValue(), 1e-7); assertEquals(jointTorque.getY(), torqueY.getDoubleValue(), 1e-7); assertEquals(jointTorque.getZ(), torqueZ.getDoubleValue(), 1e-7); }
if (robots[1].getVariable("valveClosePercentage").getValueAsDouble() >= 99.0) isValveClosed = true;