public static void setRandomTorques(Iterable<? extends OneDoFJoint> joints, Random random) { for (OneDoFJoint joint : joints) { joint.setTau(random.nextDouble()); } }
private void zeroAllTorques() { for (int i = 0; i < oneDoFJoints.size(); i++) { oneDoFJoints.get(i).setTau(0); } }
public static void setRandomTorques(Iterable<? extends OneDoFJoint> joints, Random random, double magnitude) { for (OneDoFJoint joint : joints) { joint.setTau(RandomTools.generateRandomDouble(random, magnitude)); } }
public void insertDesiredTorquesIntoOneDoFJoints(OneDoFJoint[] oneDoFJoints) { for (int i = 0; i < oneDoFJoints.length; i++) { OneDoFJoint joint = oneDoFJoints[i]; joint.setTau(lowLevelJointDataMap.get(joint.getName()).getDesiredTorque()); } }
@Override public void doTransitionOutOfAction() { for (int i = 0; i < allOneDoFjoints.length; i++) { allOneDoFjoints[i].resetDesiredAccelerationIntegrator(); allOneDoFjoints[i].setQddDesired(0.0); allOneDoFjoints[i].setTau(0.0); } }
private void controlJoint(StepprJoint joint, double qDesired, double qdDesired) { OneDoFJoint oneDoFJoint = joints.get(joint); PDController controller = controllers.get(joint); double tau = controller.compute(oneDoFJoint.getQ(), qDesired, oneDoFJoint.getQd(), qdDesired); oneDoFJoint.setTau(tau); }
@Override public void doControl(long timestamp) { for(int i = 0; i < controllers.size(); i++) { OneDoFJoint joint = joints.get(i); PDController controller = controllers.get(i); double tauFF = tauFFs.get(i).getDoubleValue(); double q_d = desiredPositions.get(i).getDoubleValue(); double qd_d = desiredVelocities.get(i).getDoubleValue(); double tau = controller.compute(joint.getQ(), q_d, joint.getQd(), qd_d) + tauFF; joint.setTau(tau); joint.setKd(damping.get(i).getDoubleValue()); } }
@Override public void doControl(long timestamp) { for(int i = 0; i < controllers.size(); i++) { OneDoFJoint joint = joints.get(i); PDController controller = controllers.get(i); double tauFF = tauFFs.get(i).getDoubleValue(); double q_d = desiredPositions.get(i).getDoubleValue(); double qd_d = desiredVelocities.get(i).getDoubleValue(); double tau = controller.compute(joint.getQ(), q_d, joint.getQd(), qd_d) + tauFF; joint.setTau(tau); joint.setKd(damping.get(i).getDoubleValue()); } }
@Override public void doTransitionIntoAction() { for (int i = 0; i < allOneDoFjoints.length; i++) { allOneDoFjoints[i].resetDesiredAccelerationIntegrator(); allOneDoFjoints[i].setQddDesired(0.0); allOneDoFjoints[i].setTau(0.0); } initialize(); }
@Override public void doControl(long timestamp) { super.doControl(timestamp); //add additional tau for(int i = 0; i < joints.size(); i++) { OneDoFJoint joint = joints.get(i); if(joint.getName().equals(funcGenJoint.getEnumValue().getSdfName())) { joint.setTau(funcGen.getValue(Conversions.nanosecondsToSeconds(timestamp))+joint.getTau()); } } }
@Override public void doAction() { bipedSupportPolygons.updateUsingContactStates(footContactStates); momentumBasedController.update(); for (int i = 0; i < allRobotJoints.length; i++) { allRobotJoints[i].setTau(0.0); lowLevelOneDoFJointDesiredDataHolder.setDesiredJointTorque(allRobotJoints[i], 0.0); } controllerCoreCommand.completeLowLevelJointData(lowLevelOneDoFJointDesiredDataHolder); }
public void computeAntiGravityJointTorques() { reset(); setFootMeasuredWrenches(); inverseDynamicsCalculator.compute(); for (int i = 0; i < allOneDoFJoints.length; i++) { OneDoFJoint oneDoFJoint = allOneDoFJoints[i]; antiGravityJointTorques.get(oneDoFJoint).set(oneDoFJoint.getTau()); oneDoFJoint.setTau(0.0); } reset(); }
@Override public void doControl(long timestamp) { for (StepprStandPrepSetpoints jointGroup : StepprStandPrepSetpoints.values) { double setpoint = desiredForces.get(jointGroup).getDoubleValue(); double damping = dampingValues.get(jointGroup).getDoubleValue(); for (int i = 0; i < jointGroup.getJoints().length; i++) { StepprJoint joint = jointGroup.getJoints()[i]; OneDoFJoint oneDoFJoint = joints.get(joint); double reflectedSetpoint = setpoint; if (i == 1) { if(jointGroup == StepprStandPrepSetpoints.HIP_Y) { reflectedSetpoint *= -1; } else { reflectedSetpoint *= jointGroup.getReflectRight(); } } oneDoFJoint.setTau(reflectedSetpoint); oneDoFJoint.setKd(damping); } } }
@Override public void writeAfterController(long timestamp) { for (int i = 0; i < oneDoFJoints.size(); i++) { OneDoFJoint oneDoFJoint = oneDoFJoints.get(i); double desiredAcceleration = oneDoFJoint.getQddDesired(); DoubleYoVariable torqueOffsetVariable = torqueOffsetMap.get(oneDoFJoint); if (resetTorqueOffsets.getBooleanValue()) torqueOffsetVariable.set(0.0); double offsetTorque = torqueOffsetVariable.getDoubleValue(); double ditherTorque = 0.0; double alpha = alphaTorqueOffset.getDoubleValue(); offsetTorque = alpha * (offsetTorque + desiredAcceleration * updateDT) + (1.0 - alpha) * offsetTorque; torqueOffsetVariable.set(offsetTorque); oneDoFJoint.setTau(oneDoFJoint.getTau() + offsetTorque + ditherTorque); } drcOutputWriter.writeAfterController(timestamp); }
@Override public void writeAfterController(long timestamp) { if (hasHighLevelControllerStateChanged.get()) { hasHighLevelControllerStateChanged.set(false); timeAtHighLevelControllerStateChange.set(TimeTools.nanoSecondstoSeconds(timestamp)); } double currentTime = TimeTools.nanoSecondstoSeconds(timestamp); double deltaTime = Math.max(currentTime - timeAtHighLevelControllerStateChange.getDoubleValue(), 0.0); if (deltaTime < slopTime.getDoubleValue()) { alphaForJointTorqueForStateChanges.set(1.0 - deltaTime / slopTime.getDoubleValue()); } else { alphaForJointTorqueForStateChanges.set(0.0); } for (int i = 0; i < allJoints.size(); i++) { OneDoFJoint oneDoFJoint = allJoints.get(i); double tau = oneDoFJoint.getTau(); AlphaFilteredYoVariable smoothedJointTorque = jointTorquesSmoothedAtStateChange.get(oneDoFJoint); smoothedJointTorque.update(tau); oneDoFJoint.setTau(smoothedJointTorque.getDoubleValue()); } drcOutputWriter.writeAfterController(timestamp); }
private void updatePDController(OneDoFJoint oneDoFJoint, double timeInCurrentState) { PDController pdController = pdControllers.get(oneDoFJoint); YoMinimumJerkTrajectory transitionSpline = transitionSplines.get(oneDoFJoint); double desiredPosition = transitionSpline.getPosition(); double desiredVelocity = transitionSpline.getVelocity(); // Setting the desired positions via SCS ui. if (manualMode.getBooleanValue()) { desiredPosition = desiredPositions.get(oneDoFJoint).getDoubleValue(); desiredVelocity = 0.0; } else { desiredPositions.get(oneDoFJoint).set(desiredPosition); } desiredVelocities.get(oneDoFJoint).set(desiredVelocity); double tau = pdController.compute(oneDoFJoint.getQ(), desiredPosition, oneDoFJoint.getQd(), desiredVelocity); tau = tau * diagnosticsPDMasterGain.getDoubleValue(); DiagnosticsWhenHangingHelper diagnosticsWhenHangingHelper = helpers.get(oneDoFJoint); if (diagnosticsWhenHangingHelper != null) { tau = diagnosticsWhenHangingHelper.getTorqueToApply(tau, adaptTorqueOffset.getBooleanValue(), maximumTorqueOffset.getDoubleValue()); } double ditherTorque = ditherAmplitude.getDoubleValue() * Math.sin(2.0 * Math.PI * ditherFrequency.getDoubleValue() * timeInCurrentState); oneDoFJoint.setTau(tau + ditherTorque); }
private void updatePDController(OneDoFJoint oneDoFJoint, double timeInCurrentState) { PDController pdController = pdControllers.get(oneDoFJoint); double desiredPosition = desiredPositions.get(oneDoFJoint).getDoubleValue(); double desiredVelocity = 0.0; double tau = pdController.compute(oneDoFJoint.getQ(), desiredPosition, oneDoFJoint.getQd(), desiredVelocity); DiagnosticsWhenHangingHelper diagnosticsWhenHangingHelper = helpers.get(oneDoFJoint); if (diagnosticsWhenHangingHelper != null) { tau = diagnosticsWhenHangingHelper.getTorqueToApply(tau, estimateTorqueOffset.getBooleanValue(), maximumTorqueOffset.getDoubleValue()); if (hasReachedMaximumTorqueOffset.getBooleanValue() && Math.abs(diagnosticsWhenHangingHelper.getTorqueOffset()) == maximumTorqueOffset.getDoubleValue()) { PrintTools.warn(this, "Reached maximum torque for at least one joint."); hasReachedMaximumTorqueOffset.set(true); } } double ditherTorque = ditherAmplitude.getDoubleValue() * Math.sin(2.0 * Math.PI * ditherFrequency.getDoubleValue() * timeInCurrentState); oneDoFJoint.setTau(tau + ditherTorque); }
positionTorqueMap.get(oneDoFJoint).set(positionTorque); oneDoFJoint.setTau(oneDoFJoint.getTau() + velocityTorque + positionTorque);
private void doIdleControl() { for (int i = 0; i < controlledJoints.size(); i++) { OneDoFJoint joint = controlledJoints.get(i); PDController jointPDController = jointPDControllerMap.get(joint); OneDoFJointQuinticTrajectoryGenerator jointTrajectory = jointTrajectories.get(joint); jointTrajectory.compute(trajectoryTimeProvider.getValue()); DoubleYoVariable jointDesiredPosition = jointDesiredPositionMap.get(joint); DoubleYoVariable jointDesiredVelocity = jointDesiredVelocityMap.get(joint); jointDesiredPosition.set(jointTrajectory.getValue()); jointDesiredVelocity.set(0.0); double q = joint.getQ(); double qDesired = jointDesiredPosition.getDoubleValue(); double qd = joint.getQd(); double qdDesired = jointDesiredVelocity.getDoubleValue(); double tauDesired = jointPDController.compute(q, qDesired, qd, qdDesired); double qdMax = qdMaxIdle.getDoubleValue(); double qddMax = qddMaxIdle.getDoubleValue(); double tauMax = tauMaxIdle.getDoubleValue(); qDesired = q + MathTools.clipToMinMax(qDesired - q, qdMax * controlDT); qdDesired = qd + MathTools.clipToMinMax(qdDesired - qd, qddMax * controlDT); tauDesired = MathTools.clipToMinMax(tauDesired, tauMax); joint.setTau(tauDesired); joint.setqDesired(qDesired); joint.setQdDesired(qdDesired); jointDesiredTauMap.get(joint).set(tauDesired); } }
private void parseLowLevelDataInOneDoFJoints() { for (int i = 0; i < controlledOneDoFJoints.length; i++) { OneDoFJoint joint = controlledOneDoFJoints[i]; LowLevelJointDataReadOnly lowLevelJointData = yoLowLevelOneDoFJointDesiredDataHolder.getLowLevelJointData(joint); if (!lowLevelJointData.hasControlMode()) throw new NullPointerException("Joint: " + joint.getName() + " has no control mode."); switch (lowLevelJointData.getControlMode()) { case FORCE_CONTROL: joint.setUnderPositionControl(false); break; case POSITION_CONTROL: joint.setUnderPositionControl(true); break; default: throw new RuntimeException("Unhandled joint control mode: " + lowLevelJointData.getControlMode()); } if (lowLevelJointData.hasDesiredPosition()) joint.setqDesired(lowLevelJointData.getDesiredPosition()); if (lowLevelJointData.hasDesiredVelocity()) joint.setQdDesired(lowLevelJointData.getDesiredVelocity()); if (lowLevelJointData.hasDesiredAcceleration()) joint.setQddDesired(lowLevelJointData.getDesiredAcceleration()); if (lowLevelJointData.hasDesiredTorque()) joint.setTau(lowLevelJointData.getDesiredTorque()); } }