@Override public double getJointTauProcessedOutput(OneDoFJoint oneDoFJoint) { return oneDoFJoint.getTau(); }
@Override public void update() { delayEstimator.update(joint.getTau(), joint.getTauMeasured()); }
@Override public double getJointTauRawOutput(OneDoFJoint oneDoFJoint) { return oneDoFJoint.getTau(); }
@Override public void update() { if (hasYoVariables) { velocityFourierAnalysis.update(velocity.getDoubleValue()); tauFourierAnalysis.update(tau.getDoubleValue()); tauDesiredFourierAnalysis.update(tauDesired.getDoubleValue()); } else { velocityFourierAnalysis.update(joint.getQd()); tauFourierAnalysis.update(joint.getTauMeasured()); tauDesiredFourierAnalysis.update(joint.getTau()); } }
public void setDesiredTorqueFromJoints(List<OneDoFJoint> joints) { for (int i = 0; i < joints.size(); i++) { OneDoFJoint joint = joints.get(i); setDesiredJointTorque(joint, joint.getTau()); } }
public void setDesiredTorqueFromJoints(OneDoFJoint[] joints) { for (int i = 0; i < joints.length; i++) { OneDoFJoint joint = joints[i]; setDesiredJointTorque(joint, joint.getTau()); } }
@Override public void write() { for (int i = 0; i < revoluteJoints.size(); i++) { ImmutablePair<OneDegreeOfFreedomJoint, OneDoFJoint> jointPair = revoluteJoints.get(i); OneDegreeOfFreedomJoint pinJoint = jointPair.getLeft(); OneDoFJoint revoluteJoint = jointPair.getRight(); pinJoint.setTau(revoluteJoint.getTau()); } }
@Override public double getJointTauProcessedOutput(OneDoFJoint oneDoFJoint) { for(int i = 0; i < sensorOneDoFJoints.length; i++) { if(sensorOneDoFJoints[i].getName() == oneDoFJoint.getName()) { return sensorOneDoFJoints[i].getTau(); } } return 0.0; }
private void updateKnees(StepprJoint leftJoint, StepprActuator leftActuator,StepprJoint rightJoint, StepprActuator rightActuator) { OneDoFJoint oneDofLeftKnee = wholeBodyControlJoints.get(leftJoint); OneDoFJoint oneDofRightKnee = wholeBodyControlJoints.get(rightJoint); leftFourbar.update(oneDofLeftKnee); rightFourbar.update(oneDofRightKnee); predictedMotorPower.get(leftActuator).set( calcPower(oneDofLeftKnee.getTau() / leftFourbar.getFourbarRatioBasedOnCalculatedInputAngle() / leftJoint.getRatio(), leftActuator.getKm())); predictedMotorPower.get(rightActuator).set( calcPower(oneDofRightKnee.getTau() / rightFourbar.getFourbarRatioBasedOnCalculatedInputAngle() / rightJoint.getRatio(), rightActuator.getKm())); }
private void updateKnees(WandererJoint leftJoint, WandererActuator leftActuator,WandererJoint rightJoint, WandererActuator rightActuator) { OneDoFJoint oneDofLeftKnee = wholeBodyControlJoints.get(leftJoint); OneDoFJoint oneDofRightKnee = wholeBodyControlJoints.get(rightJoint); leftFourbar.update(oneDofLeftKnee); rightFourbar.update(oneDofRightKnee); predictedMotorPower.get(leftActuator).set( calcPower(oneDofLeftKnee.getTau() / leftFourbar.getFourbarRatioBasedOnCalculatedInputAngle() / leftJoint.getRatio(), leftActuator.getKm())); predictedMotorPower.get(rightActuator).set( calcPower(oneDofRightKnee.getTau() / rightFourbar.getFourbarRatioBasedOnCalculatedInputAngle() / rightJoint.getRatio(), rightActuator.getKm())); }
@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()); } } }
public void updateFromModel() { for (int i = 0; i < desiredJointDataList.size(); i++) { ImmutablePair<OneDoFJoint, DesiredJointData> desiredJointData = desiredJointDataList.get(i); OneDoFJoint joint = desiredJointData.getLeft(); DesiredJointData data = desiredJointData.getRight(); data.setQddDesired(joint.getQddDesired()); data.setTauDesired(joint.getTau()); data.setPositionDesired(joint.getqDesired()); } }
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 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 updateAnkleState(OneDoFJoint ankleX, OneDoFJoint ankleY) { q_AnkleX = ankleX.getQ(); q_AnkleY = ankleY.getQ(); measuredJointVelocities.set(0,ankleX.getQd()); measuredJointVelocities.set(1,ankleY.getQd()); desiredJointTorque.set(0,ankleX.getTau()); desiredJointTorque.set(1,ankleY.getTau()); updateAnkleStateFromJointAngles(q_AnkleX, q_AnkleY); computeDesiredAcutatorTau(); }
public void setDesiredsFromOneDoFJoint(OneDoFJoint jointToExtractDesiredsFrom) { setDesiredTorque(jointToExtractDesiredsFrom.getTau()); setDesiredPosition(jointToExtractDesiredsFrom.getqDesired()); setDesiredVelocity(jointToExtractDesiredsFrom.getQdDesired()); setDesiredAcceleration(jointToExtractDesiredsFrom.getQddDesired()); setResetIntegrators(jointToExtractDesiredsFrom.getResetIntegrator()); }
public void setDesiredsFromOneDoFJoint(OneDoFJoint jointToExtractDesiredsFrom) { setDesiredTorque(jointToExtractDesiredsFrom.getTau()); setDesiredPosition(jointToExtractDesiredsFrom.getqDesired()); setDesiredVelocity(jointToExtractDesiredsFrom.getQdDesired()); setDesiredAcceleration(jointToExtractDesiredsFrom.getQddDesired()); setResetIntegrators(jointToExtractDesiredsFrom.getResetIntegrator()); }
@Override public void update() { if (hasYoVariables) { positionChecker.update(); velocityChecker.update(); tauChecker.update(); } else { positionChecker.update(joint.getQ()); velocityChecker.update(joint.getQd()); tauChecker.update(joint.getTau()); } }
public void extractAllDataFromJoints(OneDoFJoint[] joints, LowLevelJointControlMode controlMode) { for (int i = 0; i < joints.length; i++) { OneDoFJoint joint = joints[i]; setJointControlMode(joint, controlMode); setDesiredJointTorque(joint, joint.getTau()); setDesiredJointPosition(joint, joint.getqDesired()); setDesiredJointVelocity(joint, joint.getQdDesired()); setDesiredJointAcceleration(joint, joint.getQddDesired()); setResetJointIntegrators(joint, joint.getResetDesiredAccelerationIntegrator()); } }
public void update(long timestamp) { fullRobot.updateFrames(); if(rootJoint != null) { RigidBodyTransform rootTransform = rootJoint.getJointTransform3D(); rootTransform.get(tempOrientation, tempPosition); robot.setOrientation(tempOrientation); robot.setPositionInWorld(tempPosition); } for (ImmutablePair<OneDegreeOfFreedomJoint, OneDoFJoint> jointPair : revoluteJoints) { OneDegreeOfFreedomJoint pinJoint = jointPair.getLeft(); OneDoFJoint revoluteJoint = jointPair.getRight(); pinJoint.setQ(revoluteJoint.getQ()); pinJoint.setQd(revoluteJoint.getQd()); pinJoint.setTau(revoluteJoint.getTau()); } robot.setTime(robot.getTime() + updateDT); if (scs != null) { scs.tickAndUpdate(); } }