@Override public double getProportionalGain() { return proportionalGain.getValue(); }
@Override public double getPositionDeadband() { return positionDeadband.getValue(); }
public double getTime() { return time.getValue(); }
@Override public double getDerivativeGain() { return derivativeGain.getValue(); }
public double getJointCalibrationPosition(OneDoFJointBasics joint) { YoDouble yoDesiredPosition = desiredPositions.get(joint); if (yoDesiredPosition != null) return yoDesiredPosition.getValue(); else return 0.0; }
public double getFingerJointTransmissionScale(RobotSide robotSide, ValkyrieHandJointName jointName) { return sideDependentScales.get(robotSide).get(jointName).getValue(); }
private void getOneDoFJointStateFromSCS() { for (OneDoFJointBasics joint : allOneDoFJoints) { PinJoint scsJoint = (PinJoint) robot.getJoint(joint.getName()); joint.setQ(scsJoint.getQ()); joint.setQd(scsJoint.getQD()); double tau = scsJoint.getTau(); if (scsJoint.tauDamping != null) tau += scsJoint.tauDamping.getValue(); if (scsJoint.tauJointLimit != null) tau += scsJoint.tauJointLimit.getValue(); if (scsJoint.tauVelocityLimit != null) tau += scsJoint.tauVelocityLimit.getValue(); joint.setTau(tau); } }
@Override protected void computeRotationTranslation(AffineTransform transform3D) { transform3D.setIdentity(); transform3D.setScale(scale * radii.getX(), scale * radii.getY(), scale * radii.getZ()); transform3D.setRotationYawPitchRoll(yawPitchRoll.getYaw().getValue(), yawPitchRoll.getPitch().getValue(), yawPitchRoll.getRoll().getValue()); transform3D.setTranslation(position); }
@Override protected void computeRotationTranslation(AffineTransform transform3D) { transform3D.setIdentity(); double globalScale = 1.0; if (globalScaleProvider != null) { globalScale = globalScaleProvider.getValue(); } transform3D.setScale(scale * globalScale); if (isUsingYawPitchRoll()) transform3D.setRotationYawPitchRoll(yawPitchRoll.getYaw().getValue(), yawPitchRoll.getPitch().getValue(), yawPitchRoll.getRoll().getValue()); else transform3D.setRotation(quaternion); transform3D.setTranslation(position); }
public boolean isDone() { if (isEmpty()) return true; boolean isLastWaypoint = currentWaypointIndex.getIntegerValue() >= numberOfWaypoints.getIntegerValue() - 2; if (!isLastWaypoint) return false; return currentTrajectoryTime.getValue() >= waypoints.get(currentWaypointIndex.getValue() + 1).getTime(); }
@Override public boolean isDone() { if (isEmpty()) return true; boolean isLastWaypoint = currentWaypointIndex.getIntegerValue() >= numberOfWaypoints.getIntegerValue() - 2; if (!isLastWaypoint) return false; return currentTrajectoryTime.getValue() >= waypoints.get(currentWaypointIndex.getValue() + 1).getTime(); }
private void sendFootstepPlan() { FootstepPlanningToolboxOutputStatus plannerResult = this.plannerResult.getAndSet(null); FootstepDataListMessage footstepDataListMessage = plannerResult.getFootstepDataList(); footstepDataListMessage.setDefaultSwingDuration(swingTime.getValue()); footstepDataListMessage.setDefaultTransferDuration(transferTime.getDoubleValue()); footstepDataListMessage.setDestination(PacketDestination.CONTROLLER.ordinal()); footstepPublisher.publish(footstepDataListMessage); }
@Override public void compute(double time) { integratedPhaseAngle.add(2.0 * Math.PI * frequency.getValue() * controlDT); double angle = integratedPhaseAngle.getValue(); double mult = 2.0 * Math.PI * frequency.getValue(); double alpha = 0.5 * Math.sin(angle) + 0.5; double alphaDot = 0.5 * 2.0 * mult * Math.cos(angle); double alphaDDot = -0.5 * 2.0 * 2.0 * mult * mult * Math.sin(angle); limitedPointA.update(); limitedPointB.update(); position.interpolate(limitedPointA, limitedPointB, alpha); linearVelocity.sub(limitedPointB, limitedPointA); linearVelocity.scale(alphaDot); linearAcceleration.sub(limitedPointB, limitedPointA); linearAcceleration.scale(alphaDDot); }
@Override public YoDouble duplicate(YoVariableRegistry newRegistry) { DoubleParameter newParameter = new DoubleParameter(getName(), getDescription(), newRegistry, initialValue, getManualScalingMin(), getManualScalingMax()); newParameter.value.set(value.getValue()); newParameter.loadStatus = getLoadStatus(); return newParameter.value; } }
@Override public void getRMatrix(DenseMatrix64F matrixToPack) { matrixToPack.reshape(getMeasurementSize(), getMeasurementSize()); CommonOps.setIdentity(matrixToPack); double percent = (loadPercentage.getValue() - weightFractionForNoTrust.getValue()) / (weightFractionForFullTrust.getValue() - weightFractionForNoTrust.getValue()); percent = MathTools.clamp(percent, 0.0, 1.0); variance.set(maxVariance.getValue() - percent * (maxVariance.getValue() - minVariance.getValue())); CommonOps.scale(variance.getValue() * sqrtHz, matrixToPack); }
private void computeInitialConstraintOffset(double time) { double startTime = initialBlendStartTime.getDoubleValue(); double endTime = initialBlendEndTime.getDoubleValue(); time = MathTools.clamp(time - initialBlendTimeOffset.getValue(), startTime, endTime); for (int i = 0; i < 3; i++) { initialConstraintPolynomial[i].compute(time); initialConstraintPositionOffset.setElement(i, initialConstraintPolynomial[i].getPosition()); initialConstraintVelocityOffset.setElement(i, initialConstraintPolynomial[i].getVelocity()); initialConstraintAccelerationOffset.setElement(i, initialConstraintPolynomial[i].getAcceleration()); } }
private void computeFinalConstraintOffset(double time) { double startTime = finalBlendStartTime.getDoubleValue(); double endTime = finalBlendEndTime.getDoubleValue(); time = MathTools.clamp(time - finalBlendTimeOffset.getValue(), startTime, endTime); for (int i = 0; i < 3; i++) { finalConstraintPolynomial[i].compute(time); finalConstraintPositionOffset.setElement(i, finalConstraintPolynomial[i].getPosition()); finalConstraintVelocityOffset.setElement(i, finalConstraintPolynomial[i].getVelocity()); finalConstraintAccelerationOffset.setElement(i, finalConstraintPolynomial[i].getAcceleration()); } } }
@ContinuousIntegrationTest(estimatedDuration = 0.0) @Test(timeout = 30000) public void testDefaultMaximumDeflection4() { Random random = new Random(1561651L); String name = "compensator"; YoVariableRegistry registry = new YoVariableRegistry(""); YoDouble stiffness = new YoDouble("stiffness", registry); YoDouble maximumDeflection = new YoDouble("maximumDeflection", registry); YoDouble rawJointPosition = new YoDouble("rawJointPosition", registry); YoDouble jointTau = new YoDouble("jointTau", registry); ElasticityCompensatorYoVariable elasticityCompensatorYoVariable = new ElasticityCompensatorYoVariable(name, stiffness, maximumDeflection, rawJointPosition, jointTau, registry); maximumDeflection.set(0.1); for (int i = 0; i < 10000; i++) { stiffness.set(RandomNumbers.nextDouble(random, 1.0, 100000.0)); rawJointPosition.set(RandomNumbers.nextDouble(random, 100.0)); jointTau.set(RandomNumbers.nextDouble(random, 10000.0)); elasticityCompensatorYoVariable.update(); double deflectedJointPosition = rawJointPosition.getDoubleValue() - MathTools.clamp(jointTau.getDoubleValue() / stiffness.getDoubleValue(), maximumDeflection.getValue()); assertEquals(deflectedJointPosition, elasticityCompensatorYoVariable.getDoubleValue(), EPSILON); } }
@ContinuousIntegrationTest(estimatedDuration = 0.0) @Test(timeout = 30000) public void testRandomMaximumDeflection4() { Random random = new Random(1561651L); String name = "compensator"; YoVariableRegistry registry = new YoVariableRegistry(""); YoDouble stiffness = new YoDouble("stiffness", registry); YoDouble maximumDeflection = new YoDouble("maximumDeflection", registry); YoDouble rawJointPosition = new YoDouble("rawJointPosition", registry); YoDouble jointTau = new YoDouble("jointTau", registry); ElasticityCompensatorYoVariable elasticityCompensatorYoVariable = new ElasticityCompensatorYoVariable(name, stiffness, maximumDeflection, rawJointPosition, jointTau, registry); maximumDeflection.set(0.1); for (int i = 0; i < 10000; i++) { stiffness.set(RandomNumbers.nextDouble(random, 1.0, 100000.0)); rawJointPosition.set(RandomNumbers.nextDouble(random, 100.0)); jointTau.set(RandomNumbers.nextDouble(random, 10000.0)); maximumDeflection.set(RandomNumbers.nextDouble(random, 0.0, 10.0)); elasticityCompensatorYoVariable.update(); double deflectedJointPosition = rawJointPosition.getDoubleValue() - MathTools.clamp(jointTau.getDoubleValue() / stiffness.getDoubleValue(), maximumDeflection.getValue()); assertEquals(deflectedJointPosition, elasticityCompensatorYoVariable.getDoubleValue(), EPSILON); } } }