private YoGraphicLineSegment(String name, DoubleYoVariable startX, DoubleYoVariable startY, DoubleYoVariable startZ, DoubleYoVariable endX, DoubleYoVariable endY, DoubleYoVariable endZ, YoFrameVector yoFrameVector, double scaleFactor, AppearanceDefinition appearance, boolean drawArrowhead) { super(name, startX, startY, startZ, yoFrameVector.getYoX(), yoFrameVector.getYoY(), yoFrameVector.getYoZ(), scaleFactor, appearance, drawArrowhead); this.vectorX = yoFrameVector.getYoX(); this.vectorY = yoFrameVector.getYoY(); this.vectorZ = yoFrameVector.getYoZ(); this.startX = startX; this.startY = startY; this.startZ = startZ; this.endX = endX; this.endY = endY; this.endZ = endZ; // computeRotationTranslation(); }
private void verifyYoVariableNames(String wrenchSensorName, YoFrameVector forceMeasurement, YoFrameVector torqueMeasurement) { if (!forceMeasurement.getYoX().getName().contains(wrenchSensorName)) throw new RuntimeException("The force variable: " + forceMeasurement.getYoX().getName() + " may not belong to the wrench sensor: " + wrenchSensorName); if (!forceMeasurement.getYoY().getName().contains(wrenchSensorName)) throw new RuntimeException("The force variable: " + forceMeasurement.getYoY().getName() + " may not belong to the wrench sensor: " + wrenchSensorName); if (!forceMeasurement.getYoZ().getName().contains(wrenchSensorName)) throw new RuntimeException("The force variable: " + forceMeasurement.getYoZ().getName() + " may not belong to the wrench sensor: " + wrenchSensorName); if (!torqueMeasurement.getYoX().getName().contains(wrenchSensorName)) throw new RuntimeException("The torque variable: " + torqueMeasurement.getYoX().getName() + " may not belong to the wrench sensor: " + wrenchSensorName); if (!torqueMeasurement.getYoY().getName().contains(wrenchSensorName)) throw new RuntimeException("The torque variable: " + torqueMeasurement.getYoY().getName() + " may not belong to the wrench sensor: " + wrenchSensorName); if (!torqueMeasurement.getYoZ().getName().contains(wrenchSensorName)) throw new RuntimeException("The torque variable: " + torqueMeasurement.getYoZ().getName() + " may not belong to the wrench sensor: " + wrenchSensorName); } }
public void verifyYovariableNames(String imuName, YoFrameQuaternion orientation, YoFrameVector angularVelocity, YoFrameVector linearAcceleration) { if (!orientation.getYoQx().getName().contains(imuName)) throw new RuntimeException("The orientation variable: " + orientation.getYoQx().getName() + " may not belong to the IMU: " + imuName); if (!orientation.getYoQy().getName().contains(imuName)) throw new RuntimeException("The orientation variable: " + orientation.getYoQy().getName() + " may not belong to the IMU: " + imuName); if (!orientation.getYoQz().getName().contains(imuName)) throw new RuntimeException("The orientation variable: " + orientation.getYoQz().getName() + " may not belong to the IMU: " + imuName); if (!orientation.getYoQs().getName().contains(imuName)) throw new RuntimeException("The orientation variable: " + orientation.getYoQs().getName() + " may not belong to the IMU: " + imuName); if (!angularVelocity.getYoX().getName().contains(imuName)) throw new RuntimeException("The angular velocity variable: " + angularVelocity.getYoX().getName() + " may not belong to the IMU: " + imuName); if (!angularVelocity.getYoY().getName().contains(imuName)) throw new RuntimeException("The angular velocity variable: " + angularVelocity.getYoY().getName() + " may not belong to the IMU: " + imuName); if (!angularVelocity.getYoZ().getName().contains(imuName)) throw new RuntimeException("The angular velocity variable: " + angularVelocity.getYoZ().getName() + " may not belong to the IMU: " + imuName); if (!linearAcceleration.getYoX().getName().contains(imuName)) throw new RuntimeException("The linear acceleration variable: " + linearAcceleration.getYoX().getName() + " may not belong to the IMU: " + imuName); if (!linearAcceleration.getYoY().getName().contains(imuName)) throw new RuntimeException("The linear acceleration variable: " + linearAcceleration.getYoY().getName() + " may not belong to the IMU: " + imuName); if (!linearAcceleration.getYoZ().getName().contains(imuName)) throw new RuntimeException("The linear acceleration variable: " + linearAcceleration.getYoZ().getName() + " may not belong to the IMU: " + imuName); } }
@Override public void doControl() { for (int i = 0; i < efp_offsetFromRootJoint.size(); i++) { initialPositions.get(i).setZ(desiredHeight.getDoubleValue()); ExternalForcePoint efp = externalForcePoints.get(i); efp.getYoPosition().get(proportionalTerm); proportionalTerm.sub(initialPositions.get(i)); proportionalTerm.scale(-holdPelvisKp.getDoubleValue()); // proportionalTerm.setZ(Math.max(proportionalTerm.getZ(), 0.0)); efp.getYoVelocity().get(derivativeTerm); derivativeTerm.scale(-holdPelvisKv.getDoubleValue()); pdControlOutput.add(proportionalTerm, derivativeTerm); efp.setForce(pdControlOutput); efp.getYoForce().getYoZ().add(robotWeight / efp_offsetFromRootJoint.size()); efp_positionViz.get(i).update(); } }
@Override public void doControl() { for (int i = 0; i < efp_offsetFromRootJoint.size(); i++) { initialPositions.get(i).setZ(desiredHeight.getDoubleValue()); ExternalForcePoint efp = externalForcePoints.get(i); efp.getYoPosition().get(proportionalTerm); proportionalTerm.sub(initialPositions.get(i)); proportionalTerm.scale(-holdPelvisKp.getDoubleValue()); // proportionalTerm.setZ(Math.max(proportionalTerm.getZ(), 0.0)); efp.getYoVelocity().get(derivativeTerm); derivativeTerm.scale(-holdPelvisKv.getDoubleValue()); pdControlOutput.add(proportionalTerm, derivativeTerm); efp.setForce(pdControlOutput); efp.getYoForce().getYoZ().add(robotWeight / efp_offsetFromRootJoint.size()); efp_positionViz.get(i).update(); } }
public YoGraphicVector(String name, YoFramePoint startPoint, YoFrameVector frameVector, double scale, AppearanceDefinition appearance, boolean drawArrowhead) { this(name, startPoint.getYoX(), startPoint.getYoY(), startPoint.getYoZ(), frameVector.getYoX(), frameVector.getYoY(), frameVector.getYoZ(), scale, appearance, drawArrowhead); if ((!startPoint.getReferenceFrame().isWorldFrame()) || (!frameVector.getReferenceFrame().isWorldFrame())) { System.err.println("Warning: Should be in a World Frame to create a DynamicGraphicVector. startPoint = " + startPoint + ", frameVector = " + frameVector); } }
public YoGraphicCylinder(String name, YoFramePoint startPoint, YoFrameVector frameVector, AppearanceDefinition appearance, double lineThickness) { this(name, startPoint.getYoX(), startPoint.getYoY(), startPoint.getYoZ(), frameVector.getYoX(), frameVector.getYoY(), frameVector.getYoZ(), appearance, lineThickness); if ((!startPoint.getReferenceFrame().isWorldFrame()) || (!frameVector.getReferenceFrame().isWorldFrame())) { System.err.println("Warning: Should be in a World Frame to create a DynamicGraphicCylinder. startPoint = " + startPoint + ", frameVector = " + frameVector); } }
public static AverageSampleYoFrameVector createAverageSampleYoFrameVector(String namePrefix, String nameSuffix, YoVariableRegistry registry, YoFrameVector dataSource, ReferenceFrame referenceFrame) { AverageSampleDoubleYoVariable x = new AverageSampleDoubleYoVariable(YoFrameVariableNameTools.createXName(namePrefix, nameSuffix), dataSource.getYoX(), registry); AverageSampleDoubleYoVariable y = new AverageSampleDoubleYoVariable(YoFrameVariableNameTools.createYName(namePrefix, nameSuffix), dataSource.getYoY(), registry); AverageSampleDoubleYoVariable z = new AverageSampleDoubleYoVariable(YoFrameVariableNameTools.createZName(namePrefix, nameSuffix), dataSource.getYoZ(), registry); AverageSampleYoFrameVector ret = new AverageSampleYoFrameVector(x, y, z, referenceFrame); return ret; }
public static BacklashCompensatingVelocityYoFrameVector createBacklashCompensatingVelocityYoFrameVector(String namePrefix, String nameSuffix, DoubleYoVariable alpha, double dt, DoubleYoVariable slopTime, YoVariableRegistry registry, YoFrameVector yoFrameVectorToDifferentiate) { BacklashCompensatingVelocityYoVariable xDot = new BacklashCompensatingVelocityYoVariable(YoFrameVariableNameTools.createXName(namePrefix, nameSuffix), "", alpha, yoFrameVectorToDifferentiate.getYoX(), dt, slopTime, registry); BacklashCompensatingVelocityYoVariable yDot = new BacklashCompensatingVelocityYoVariable(YoFrameVariableNameTools.createYName(namePrefix, nameSuffix), "", alpha, yoFrameVectorToDifferentiate.getYoY(), dt, slopTime, registry); BacklashCompensatingVelocityYoVariable zDot = new BacklashCompensatingVelocityYoVariable(YoFrameVariableNameTools.createZName(namePrefix, nameSuffix), "", alpha, yoFrameVectorToDifferentiate.getYoZ(), dt, slopTime, registry); BacklashCompensatingVelocityYoFrameVector ret = new BacklashCompensatingVelocityYoFrameVector(xDot, yDot, zDot, registry, yoFrameVectorToDifferentiate); return ret; }
public static AlphaFilteredYoFrameVector createAlphaFilteredYoFrameVector(String namePrefix, String nameSuffix, YoVariableRegistry registry, double alpha, YoFrameVector unfilteredVector) { // alpha is a double AlphaFilteredYoVariable x = new AlphaFilteredYoVariable(YoFrameVariableNameTools.createXName(namePrefix, nameSuffix), registry, alpha, unfilteredVector.getYoX()); AlphaFilteredYoVariable y = new AlphaFilteredYoVariable(YoFrameVariableNameTools.createYName(namePrefix, nameSuffix), registry, alpha, unfilteredVector.getYoY()); AlphaFilteredYoVariable z = new AlphaFilteredYoVariable(YoFrameVariableNameTools.createZName(namePrefix, nameSuffix), registry, alpha, unfilteredVector.getYoZ()); AlphaFilteredYoFrameVector ret = new AlphaFilteredYoFrameVector(x, y, z, unfilteredVector.getReferenceFrame()); return ret; }
public static AlphaFilteredYoFrameVector createAlphaFilteredYoFrameVector(String namePrefix, String nameSuffix, YoVariableRegistry registry, DoubleYoVariable alpha, YoFrameVector unfilteredVector) { // alpha is a YoVariable AlphaFilteredYoVariable x = new AlphaFilteredYoVariable(YoFrameVariableNameTools.createXName(namePrefix, nameSuffix), registry, alpha, unfilteredVector.getYoX()); AlphaFilteredYoVariable y = new AlphaFilteredYoVariable(YoFrameVariableNameTools.createYName(namePrefix, nameSuffix), registry, alpha, unfilteredVector.getYoY()); AlphaFilteredYoVariable z = new AlphaFilteredYoVariable(YoFrameVariableNameTools.createZName(namePrefix, nameSuffix), registry, alpha, unfilteredVector.getYoZ()); AlphaFilteredYoFrameVector ret = new AlphaFilteredYoFrameVector(x, y, z, unfilteredVector.getReferenceFrame()); return ret; }
public static SimpleMovingAverageFilteredYoFrameVector createSimpleMovingAverageFilteredYoFrameVector(String namePrefix, String nameSuffix, int windowSize, YoFrameVector unfilteredVector, YoVariableRegistry registry) { String xName = createXName(namePrefix, nameSuffix); String yName = createYName(namePrefix, nameSuffix); String zName = createZName(namePrefix, nameSuffix); SimpleMovingAverageFilteredYoVariable x = new SimpleMovingAverageFilteredYoVariable(xName, windowSize, unfilteredVector.getYoX(), registry); SimpleMovingAverageFilteredYoVariable y = new SimpleMovingAverageFilteredYoVariable(yName, windowSize, unfilteredVector.getYoY(), registry); SimpleMovingAverageFilteredYoVariable z = new SimpleMovingAverageFilteredYoVariable(zName, windowSize, unfilteredVector.getYoZ(), registry); return new SimpleMovingAverageFilteredYoFrameVector(x, y, z, unfilteredVector.getReferenceFrame()); }
public static TimestampedVelocityYoFrameVector createFilteredVelocityYoFrameVector(String namePrefix, String nameSuffix, DoubleYoVariable timestamp, double dt, YoVariableRegistry registry, YoFrameVector yoFrameVectorToDifferentiate, double epsilonChange) { TimestampedVelocityYoVariable xDot = new TimestampedVelocityYoVariable(YoFrameVariableNameTools.createXName(namePrefix, nameSuffix), "", yoFrameVectorToDifferentiate.getYoX(), timestamp, registry, epsilonChange); TimestampedVelocityYoVariable yDot = new TimestampedVelocityYoVariable(YoFrameVariableNameTools.createYName(namePrefix, nameSuffix), "", yoFrameVectorToDifferentiate.getYoY(), timestamp, registry, epsilonChange); TimestampedVelocityYoVariable zDot = new TimestampedVelocityYoVariable(YoFrameVariableNameTools.createZName(namePrefix, nameSuffix), "", yoFrameVectorToDifferentiate.getYoZ(), timestamp, registry, epsilonChange); TimestampedVelocityYoFrameVector ret = new TimestampedVelocityYoFrameVector(xDot, yDot, zDot, yoFrameVectorToDifferentiate.getReferenceFrame()); return ret; }
public static RateLimitedYoFrameVector createRateLimitedYoFrameVector(String namePrefix, String nameSuffix, YoVariableRegistry registry, double maxRate, double dt, YoFrameVector unfilteredVector) { // alpha is a double RateLimitedYoVariable x = new RateLimitedYoVariable(YoFrameVariableNameTools.createXName(namePrefix, nameSuffix), registry, maxRate, unfilteredVector.getYoX(), dt); RateLimitedYoVariable y = new RateLimitedYoVariable(YoFrameVariableNameTools.createYName(namePrefix, nameSuffix), registry, maxRate, unfilteredVector.getYoY(), dt); RateLimitedYoVariable z = new RateLimitedYoVariable(YoFrameVariableNameTools.createZName(namePrefix, nameSuffix), registry, maxRate, unfilteredVector.getYoZ(), dt); RateLimitedYoFrameVector ret = new RateLimitedYoFrameVector(x, y, z, unfilteredVector.getReferenceFrame()); return ret; }
public static RateLimitedYoFrameVector createRateLimitedYoFrameVector(String namePrefix, String nameSuffix, YoVariableRegistry registry, DoubleYoVariable maxRate, double dt, YoFrameVector unfilteredVector) { // alpha is a YoVariable RateLimitedYoVariable x = new RateLimitedYoVariable(YoFrameVariableNameTools.createXName(namePrefix, nameSuffix), registry, maxRate, unfilteredVector.getYoX(), dt); RateLimitedYoVariable y = new RateLimitedYoVariable(YoFrameVariableNameTools.createYName(namePrefix, nameSuffix), registry, maxRate, unfilteredVector.getYoY(), dt); RateLimitedYoVariable z = new RateLimitedYoVariable(YoFrameVariableNameTools.createZName(namePrefix, nameSuffix), registry, maxRate, unfilteredVector.getYoZ(), dt); RateLimitedYoFrameVector ret = new RateLimitedYoFrameVector(x, y, z, unfilteredVector.getReferenceFrame()); return ret; }
public static FilteredVelocityYoFrameVector createFilteredVelocityYoFrameVector(String namePrefix, String nameSuffix, DoubleYoVariable alpha, double dt, YoVariableRegistry registry, YoFrameVector yoFrameVectorToDifferentiate) { FilteredVelocityYoVariable xDot = new FilteredVelocityYoVariable(YoFrameVariableNameTools.createXName(namePrefix, nameSuffix), "", alpha, yoFrameVectorToDifferentiate.getYoX(), dt, registry); FilteredVelocityYoVariable yDot = new FilteredVelocityYoVariable(YoFrameVariableNameTools.createYName(namePrefix, nameSuffix), "", alpha, yoFrameVectorToDifferentiate.getYoY(), dt, registry); FilteredVelocityYoVariable zDot = new FilteredVelocityYoVariable(YoFrameVariableNameTools.createZName(namePrefix, nameSuffix), "", alpha, yoFrameVectorToDifferentiate.getYoZ(), dt, registry); FilteredVelocityYoFrameVector ret = new FilteredVelocityYoFrameVector(xDot, yDot, zDot, alpha, dt, registry, yoFrameVectorToDifferentiate.getReferenceFrame()); return ret; }