public YoPoint3D(String namePrefix, String nameSuffix, YoVariableRegistry registry) { x = new YoDouble(YoFrameVariableNameTools.createXName(namePrefix, nameSuffix), registry); y = new YoDouble(YoFrameVariableNameTools.createYName(namePrefix, nameSuffix), registry); z = new YoDouble(YoFrameVariableNameTools.createZName(namePrefix, nameSuffix), registry); }
public static YoFrameQuaternion createYoOrientation(final ReferenceFrameHolder referenceFrameHolder, String namePrefix, String nameSuffix, YoVariableRegistry registry) { return new YoFrameQuaternion(createName(namePrefix, "orientation", ""), nameSuffix, null, registry) { @Override public ReferenceFrame getReferenceFrame() { return referenceFrameHolder.getReferenceFrame(); } }; }
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 YoFrameQuaternion findYoFrameQuaternion(String quaternionPrefix, String quaternionSuffix, ReferenceFrame quaternionFrame) { DoubleYoVariable qx = (DoubleYoVariable) scs.getVariable(createQxName(quaternionPrefix, quaternionSuffix)); DoubleYoVariable qy = (DoubleYoVariable) scs.getVariable(createQyName(quaternionPrefix, quaternionSuffix)); DoubleYoVariable qz = (DoubleYoVariable) scs.getVariable(createQzName(quaternionPrefix, quaternionSuffix)); DoubleYoVariable qs = (DoubleYoVariable) scs.getVariable(createQsName(quaternionPrefix, quaternionSuffix)); if (qx == null || qy == null || qz == null || qs == null) return null; else return new YoFrameQuaternion(qx, qy, qz, qs, quaternionFrame); } }
public static RateLimitedYoFrameVector2d createRateLimitedYoFrameVector2d(String namePrefix, String nameSuffix, YoVariableRegistry registry, YoDouble maxRate, double dt, ReferenceFrame referenceFrame) { RateLimitedYoVariable x = new RateLimitedYoVariable(YoFrameVariableNameTools.createXName(namePrefix, nameSuffix), registry, maxRate, dt); RateLimitedYoVariable y = new RateLimitedYoVariable(YoFrameVariableNameTools.createYName(namePrefix, nameSuffix), registry, maxRate, dt); RateLimitedYoFrameVector2d ret = new RateLimitedYoFrameVector2d(x, y, referenceFrame); return ret; }
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 YoFrameQuaternion(String namePrefix, String nameSuffix, ReferenceFrame referenceFrame, YoVariableRegistry registry) { this.namePrefix = namePrefix; this.nameSuffix = nameSuffix; this.qx = new DoubleYoVariable(createQxName(namePrefix, nameSuffix), registry); this.qy = new DoubleYoVariable(createQyName(namePrefix, nameSuffix), registry); this.qz = new DoubleYoVariable(createQzName(namePrefix, nameSuffix), registry); this.qs = new DoubleYoVariable(createQsName(namePrefix, nameSuffix), registry); this.referenceFrame = referenceFrame; qs.set(1.0); }
public static BetaFilteredYoFrameVector2d createBetaFilteredYoFrameVector2d(String namePrefix, String nameSuffix, YoVariableRegistry registry, int beta, ReferenceFrame referenceFrame) { // beta is a int BetaFilteredYoVariable x = new BetaFilteredYoVariable(YoFrameVariableNameTools.createXName(namePrefix, nameSuffix), registry, beta); BetaFilteredYoVariable y = new BetaFilteredYoVariable(YoFrameVariableNameTools.createYName(namePrefix, nameSuffix), registry, beta); BetaFilteredYoFrameVector2d ret = new BetaFilteredYoFrameVector2d(x, y, referenceFrame); return ret; }
public YoPoint3D(String namePrefix, YoVariableRegistry registry) { x = new YoDouble(YoFrameVariableNameTools.createXName(namePrefix, ""), registry); y = new YoDouble(YoFrameVariableNameTools.createYName(namePrefix, ""), registry); z = new YoDouble(YoFrameVariableNameTools.createZName(namePrefix, ""), registry); }
public YoFrameQuaternion findYoFrameQuaternion(String quaternionPrefix, String quaternionSuffix, ReferenceFrame quaternionFrame) { YoDouble qx = (YoDouble) scs.getVariable(createQxName(quaternionPrefix, quaternionSuffix)); YoDouble qy = (YoDouble) scs.getVariable(createQyName(quaternionPrefix, quaternionSuffix)); YoDouble qz = (YoDouble) scs.getVariable(createQzName(quaternionPrefix, quaternionSuffix)); YoDouble qs = (YoDouble) scs.getVariable(createQsName(quaternionPrefix, quaternionSuffix)); if (qx == null || qy == null || qz == null || qs == null) return null; else return new YoFrameQuaternion(qx, qy, qz, qs, quaternionFrame); } }
public static BetaFilteredYoFramePoint2d createBetaFilteredYoFramePoint2d(String namePrefix, String nameSuffix, YoVariableRegistry registry, int beta, ReferenceFrame referenceFrame) { // beta is a int BetaFilteredYoVariable x = new BetaFilteredYoVariable(YoFrameVariableNameTools.createXName(namePrefix, nameSuffix), registry, beta); BetaFilteredYoVariable y = new BetaFilteredYoVariable(YoFrameVariableNameTools.createYName(namePrefix, nameSuffix), registry, beta); BetaFilteredYoFramePoint2d ret = new BetaFilteredYoFramePoint2d(x, y, referenceFrame); return ret; }
public static YoFrameQuaternion createYoOrientation(final ReferenceFrameHolder referenceFrameHolder, String namePrefix, String nameSuffix, YoVariableRegistry registry) { return new YoFrameQuaternion(createName(namePrefix, "orientation", ""), nameSuffix, null, registry) { @Override public ReferenceFrame getReferenceFrame() { return referenceFrameHolder.getReferenceFrame(); } }; }
public static RateLimitedYoFrameVector createRateLimitedYoFrameVector(String namePrefix, String nameSuffix, YoVariableRegistry registry, double maxRate, double dt, ReferenceFrame referenceFrame) { // alpha is a double RateLimitedYoVariable x = new RateLimitedYoVariable(YoFrameVariableNameTools.createXName(namePrefix, nameSuffix), registry, maxRate, dt); RateLimitedYoVariable y = new RateLimitedYoVariable(YoFrameVariableNameTools.createYName(namePrefix, nameSuffix), registry, maxRate, dt); RateLimitedYoVariable z = new RateLimitedYoVariable(YoFrameVariableNameTools.createZName(namePrefix, nameSuffix), registry, maxRate, dt); RateLimitedYoFrameVector ret = new RateLimitedYoFrameVector(x, y, z, referenceFrame); return ret; }
public YoFrameQuaternion findYoFrameQuaternion(String quaternionPrefix, String quaternionSuffix, ReferenceFrame quaternionFrame) { DoubleYoVariable qx = (DoubleYoVariable) scs.getVariable(createQxName(quaternionPrefix, quaternionSuffix)); DoubleYoVariable qy = (DoubleYoVariable) scs.getVariable(createQyName(quaternionPrefix, quaternionSuffix)); DoubleYoVariable qz = (DoubleYoVariable) scs.getVariable(createQzName(quaternionPrefix, quaternionSuffix)); DoubleYoVariable qs = (DoubleYoVariable) scs.getVariable(createQsName(quaternionPrefix, quaternionSuffix)); if (qx == null || qy == null || qz == null || qs == null) return null; else return new YoFrameQuaternion(qx, qy, qz, qs, quaternionFrame); } }
public static BetaFilteredYoFramePoint2d createBetaFilteredYoFramePoint2d(String namePrefix, String nameSuffix, YoVariableRegistry registry, int beta, YoFramePoint2d unfilteredPoint) { // beta is a int BetaFilteredYoVariable x = new BetaFilteredYoVariable(YoFrameVariableNameTools.createXName(namePrefix, nameSuffix), registry, beta, unfilteredPoint.getYoX()); BetaFilteredYoVariable y = new BetaFilteredYoVariable(YoFrameVariableNameTools.createYName(namePrefix, nameSuffix), registry, beta, unfilteredPoint.getYoY()); BetaFilteredYoFramePoint2d ret = new BetaFilteredYoFramePoint2d(x, y, unfilteredPoint.getReferenceFrame()); return ret; }
public static YoFramePoint createYoPosition(final ReferenceFrameHolder referenceFrameHolder, String namePrefix, String nameSuffix, YoVariableRegistry registry) { return new YoFramePoint(createName(namePrefix, "position", ""), nameSuffix, null, registry) { @Override public ReferenceFrame getReferenceFrame() { return referenceFrameHolder.getReferenceFrame(); } }; }
public static FilteredVelocityYoFrameVector createFilteredVelocityYoFrameVector(String namePrefix, String nameSuffix, DoubleYoVariable alpha, double dt, YoVariableRegistry registry, ReferenceFrame referenceFrame) { FilteredVelocityYoVariable xDot = new FilteredVelocityYoVariable(YoFrameVariableNameTools.createXName(namePrefix, nameSuffix), "", alpha, dt, registry); FilteredVelocityYoVariable yDot = new FilteredVelocityYoVariable(YoFrameVariableNameTools.createYName(namePrefix, nameSuffix), "", alpha, dt, registry); FilteredVelocityYoVariable zDot = new FilteredVelocityYoVariable(YoFrameVariableNameTools.createZName(namePrefix, nameSuffix), "", alpha, dt, registry); FilteredVelocityYoFrameVector ret = new FilteredVelocityYoFrameVector(xDot, yDot, zDot, alpha, dt, registry, referenceFrame); return ret; }
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); }
public static AlphaFilteredYoFramePoint2d createAlphaFilteredYoFramePoint2d(String namePrefix, String nameSuffix, YoVariableRegistry registry, double alpha, YoFramePoint2d unfilteredPoint) { // alpha is a double AlphaFilteredYoVariable x = new AlphaFilteredYoVariable(YoFrameVariableNameTools.createXName(namePrefix, nameSuffix), registry, alpha, unfilteredPoint.getYoX()); AlphaFilteredYoVariable y = new AlphaFilteredYoVariable(YoFrameVariableNameTools.createYName(namePrefix, nameSuffix), registry, alpha, unfilteredPoint.getYoY()); AlphaFilteredYoFramePoint2d ret = new AlphaFilteredYoFramePoint2d(x, y, unfilteredPoint.getReferenceFrame()); return ret; }
public static YoFrameVector createYoLinearVelocity(final ReferenceFrameHolder referenceFrameHolder, String namePrefix, String nameSuffix, YoVariableRegistry registry) { return new YoFrameVector(createName(namePrefix, "linearVelocity", ""), nameSuffix, null, registry) { @Override public ReferenceFrame getReferenceFrame() { return referenceFrameHolder.getReferenceFrame(); } }; }