public YoVariable<?> getYoVariable(String name) { return scs.getVariable(name); }
private static <T extends YoVariable<T>> T getYoVariable(SimulationConstructionSet scs, String name, String namespace, Class<T> clazz) { YoVariable<?> uncheckedVariable = scs.getVariable(namespace, name); if (uncheckedVariable == null) throw new RuntimeException("Could not find yo variable: " + namespace + "/" + name + "."); if (!clazz.isInstance(uncheckedVariable)) throw new RuntimeException("YoVariable " + name + " is not of type " + clazz.getSimpleName()); return clazz.cast(uncheckedVariable); }
private static <T extends YoVariable<T>> T getYoVariable(SimulationConstructionSet scs, String name, String namespace, Class<T> clazz) { YoVariable<?> uncheckedVariable = scs.getVariable(namespace, name); if (uncheckedVariable == null) throw new RuntimeException("Could not find yo variable: " + namespace + "/" + name + "."); if (!clazz.isInstance(uncheckedVariable)) throw new RuntimeException("YoVariable " + name + " is not of type " + clazz.getSimpleName()); return clazz.cast(uncheckedVariable); }
@SuppressWarnings("unchecked") public RigidBodyControlMode findControllerState(SimulationConstructionSet scs) { String namespace = "utorsoManager"; String state = namespace + "State"; return ((YoEnum<RigidBodyControlMode>) scs.getVariable(namespace, state)).getEnumValue(); }
@SuppressWarnings("unchecked") public static RigidBodyControlMode findControllerState(String bodyName, SimulationConstructionSet scs) { String managerName = bodyName + "Manager"; return ((YoEnum<RigidBodyControlMode>) scs.getVariable(managerName, managerName + "CurrentState")).getEnumValue(); }
public CutBufferToDiagnosticsStateButton(SimulationConstructionSet simulationConstructionSet) { super("Cut Buffer"); this.simulationConstructionSet = simulationConstructionSet; diagnosticsState = (YoEnum<DiagnosticsWhenHangingState>) simulationConstructionSet.getVariable("DiagnosticsState"); this.addActionListener(this); }
@SuppressWarnings("unchecked") private static RobotSide findUpcomingFootstepSide(int index, SimulationConstructionSet scs) { return ((YoEnum<RobotSide>)scs.getVariable(WalkingMessageHandler.class.getSimpleName(), "upcomingFoostepSide" + index)).getEnumValue(); }
public YoFramePoint findYoFramePoint(String pointPrefix, String pointSuffix, ReferenceFrame pointFrame) { DoubleYoVariable x = (DoubleYoVariable) scs.getVariable(createXName(pointPrefix, pointSuffix)); DoubleYoVariable y = (DoubleYoVariable) scs.getVariable(createYName(pointPrefix, pointSuffix)); DoubleYoVariable z = (DoubleYoVariable) scs.getVariable(createZName(pointPrefix, pointSuffix)); if (x == null || y == null || z == null) return null; else return new YoFramePoint(x, y, z, pointFrame); }
public YoFrameVector findYoFrameVector(String vectorPrefix, String vectorSuffix, ReferenceFrame vectorFrame) { DoubleYoVariable x = (DoubleYoVariable) scs.getVariable(createXName(vectorPrefix, vectorSuffix)); DoubleYoVariable y = (DoubleYoVariable) scs.getVariable(createYName(vectorPrefix, vectorSuffix)); DoubleYoVariable z = (DoubleYoVariable) scs.getVariable(createZName(vectorPrefix, vectorSuffix)); if (x == null || y == null || z == null) return null; else return new YoFrameVector(x, y, z, vectorFrame); }
public LogDataProcessorWrapper(SimulationConstructionSet scs) { scs.addYoVariableRegistry(logDataProcessorRegistry); scs.addScript(this); controllerTimerCount = (YoLong) scs.getVariable(DRCControllerThread.class.getSimpleName(), "controllerTimerCount"); haveFoundControllerTimerVariable = controllerTimerCount != null; if (!haveFoundControllerTimerVariable) { System.err.println("Could not find controller timer variable, running processors at log data rate"); } }
private static YoFramePose3D findYoFramePose3D(String nameSpace, String namePrefix, String nameSuffix, SimulationConstructionSet scs) { YoDouble x = (YoDouble) scs.getVariable(nameSpace, YoFrameVariableNameTools.createXName(namePrefix, nameSuffix)); YoDouble y = (YoDouble) scs.getVariable(nameSpace, YoFrameVariableNameTools.createYName(namePrefix, nameSuffix)); YoDouble z = (YoDouble) scs.getVariable(nameSpace, YoFrameVariableNameTools.createZName(namePrefix, nameSuffix)); YoFramePoint3D position = new YoFramePoint3D(x, y, z, ReferenceFrame.getWorldFrame()); YoDouble qx = (YoDouble) scs.getVariable(nameSpace, YoFrameVariableNameTools.createQxName(namePrefix, nameSuffix)); YoDouble qy = (YoDouble) scs.getVariable(nameSpace, YoFrameVariableNameTools.createQyName(namePrefix, nameSuffix)); YoDouble qz = (YoDouble) scs.getVariable(nameSpace, YoFrameVariableNameTools.createQzName(namePrefix, nameSuffix)); YoDouble qs = (YoDouble) scs.getVariable(nameSpace, YoFrameVariableNameTools.createQsName(namePrefix, nameSuffix)); YoFrameQuaternion orientation = new YoFrameQuaternion(qx, qy, qz, qs, ReferenceFrame.getWorldFrame()); return new YoFramePose3D(position, orientation); }
public static int findControllerNumberOfWaypointsInGenerator(SimulationConstructionSet scs, RigidBodyBasics body, String prefix) { String bodyName = body.getName(); int numberOfWaypoints = ((YoInteger) scs.getVariable(bodyName + "TaskspaceControlModule", bodyName + prefix + "TaskspaceNumberOfPointsInGenerator")).getIntegerValue(); return numberOfWaypoints; }
public static double[] findControllerDesiredJointAccelerations(String bodyName, RobotSide robotSide, OneDoFJointBasics[] armJoints, SimulationConstructionSet scs) { double[] qdd_ds = new double[armJoints.length]; String nameSpace = bodyName + "UserControlModule"; for (int i = 0; i < armJoints.length; i++) { String variable = bodyName + "UserMode_" + armJoints[i].getName() + "_qdd_d"; qdd_ds[i] = scs.getVariable(nameSpace, variable).getValueAsDouble(); } return qdd_ds; }
public double[] findControllerDesiredJointAccelerations(OneDoFJointBasics[] joints, SimulationConstructionSet scs) { double[] qdd_ds = new double[joints.length]; String prefix = "utorsoUserMode"; for (int i = 0; i < joints.length; i++) { String name = prefix + "_" + joints[i].getName() + "_qdd_d"; qdd_ds[i] = scs.getVariable(name).getValueAsDouble(); } return qdd_ds; }
public static Quaternion findQuat4d(String nameSpace, String prefix, String suffix, SimulationConstructionSet scs) { double x = scs.getVariable(nameSpace, YoFrameVariableNameTools.createQxName(prefix, suffix)).getValueAsDouble(); double y = scs.getVariable(nameSpace, YoFrameVariableNameTools.createQyName(prefix, suffix)).getValueAsDouble(); double z = scs.getVariable(nameSpace, YoFrameVariableNameTools.createQzName(prefix, suffix)).getValueAsDouble(); double s = scs.getVariable(nameSpace, YoFrameVariableNameTools.createQsName(prefix, suffix)).getValueAsDouble(); return new Quaternion(x, y, z, s); }
private void assertWeightsMatch(double xWeight, double yWeight, double zWeight, RigidBodyBasics rigidBody, SimulationConstructionSet scs) { String prefix = rigidBody.getName() + "TaskspaceOrientation"; YoDouble angularWeightX = (YoDouble) scs.getVariable(prefix + "CurrentWeightX"); YoDouble angularWeightY = (YoDouble) scs.getVariable(prefix + "CurrentWeightY"); YoDouble angularWeightZ = (YoDouble) scs.getVariable(prefix + "CurrentWeightZ"); assertEquals(xWeight, angularWeightX.getDoubleValue(), 1e-8); assertEquals(yWeight, angularWeightY.getDoubleValue(), 1e-8); assertEquals(zWeight, angularWeightZ.getDoubleValue(), 1e-8); }
public static Tuple3DBasics findTuple3d(String nameSpace, String prefix, String suffix, SimulationConstructionSet scs) { Tuple3DBasics tuple3d = new Point3D(); tuple3d.setX(scs.getVariable(nameSpace, YoFrameVariableNameTools.createXName(prefix, suffix)).getValueAsDouble()); tuple3d.setY(scs.getVariable(nameSpace, YoFrameVariableNameTools.createYName(prefix, suffix)).getValueAsDouble()); tuple3d.setZ(scs.getVariable(nameSpace, YoFrameVariableNameTools.createZName(prefix, suffix)).getValueAsDouble()); return tuple3d; }
public static Tuple2DBasics findTuple2d(String nameSpace, String prefix, String suffix, SimulationConstructionSet scs) { Tuple2DBasics tuple2d = new Point2D(); tuple2d.setX(scs.getVariable(nameSpace, YoFrameVariableNameTools.createXName(prefix, suffix)).getValueAsDouble()); tuple2d.setY(scs.getVariable(nameSpace, YoFrameVariableNameTools.createYName(prefix, suffix)).getValueAsDouble()); return tuple2d; }
private YoDouble getPelvisOrientationErrorVariableName(SimulationConstructionSet scs, FullHumanoidRobotModel fullRobotModel) { String pelvisName = fullRobotModel.getPelvis().getName(); String namePrefix = pelvisName + FeedbackControllerDataReadOnly.Type.ERROR.getName() + FeedbackControllerDataReadOnly.Space.ROTATION_VECTOR.getName(); String varName = YoFrameVariableNameTools.createZName(namePrefix, ""); return (YoDouble) scs.getVariable(FeedbackControllerToolbox.class.getSimpleName(), varName); } }
public static SimpleSO3TrajectoryPoint findOrientationTrajectoryPoint(String bodyName, int index, SimulationConstructionSet scs) { String orientationTrajectoryName = bodyName + MultipleWaypointsOrientationTrajectoryGenerator.class.getSimpleName(); String suffix = "AtWaypoint" + index; String timeName = bodyName + "Time"; String orientationName = bodyName + "Orientation"; String angularVelocityName = bodyName + "AngularVelocity"; SimpleSO3TrajectoryPoint simpleSO3TrajectoryPoint = new SimpleSO3TrajectoryPoint(); simpleSO3TrajectoryPoint.setTime(scs.getVariable(orientationTrajectoryName, timeName + suffix).getValueAsDouble()); simpleSO3TrajectoryPoint.setOrientation(findQuat4d(orientationTrajectoryName, orientationName, suffix, scs)); simpleSO3TrajectoryPoint.setAngularVelocity(findVector3d(orientationTrajectoryName, angularVelocityName, suffix, scs)); return simpleSO3TrajectoryPoint; }