public void setJointState(ArrayList<OneDoFJoint> newJointData) { if (newJointData.size() != jointAngles.length) throw new RuntimeException("Array size does not match"); for (int i = 0; i < jointAngles.length; i++) { jointAngles[i] = (float) newJointData.get(i).getQ(); jointVelocities[i] = (float) newJointData.get(i).getQd(); jointTorques[i] = (float) newJointData.get(i).getTauMeasured(); } }
@Override public void update() { double threshold = torqueThreshold.getDoubleValue() * signum; double torque = joint.getTauMeasured() * signum; jointTorque.set(joint.getTauMeasured()); touchdownDetected.set(torque > threshold); } }
@Override public void update() { delayEstimator.update(joint.getTau(), joint.getTauMeasured()); }
@Override public void setJointPositionVelocityAndAcceleration(InverseDynamicsJoint originalJoint) { OneDoFJoint oneDoFOriginalJoint = checkAndGetAsOneDoFJoint(originalJoint); setQ(oneDoFOriginalJoint.getQ()); setQd(oneDoFOriginalJoint.getQd()); setQdd(oneDoFOriginalJoint.getQdd()); setTauMeasured(oneDoFOriginalJoint.getTauMeasured()); setEnabled(oneDoFOriginalJoint.isEnabled()); }
@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()); } }
@Override public void update() { for(int i = 0; i < legOneDoFJoints.size(); i++) { OneDoFJoint oneDoFJoint = legOneDoFJoints.get(i); jointTorques.set(i, 0, oneDoFJoint.getTauMeasured()); } footJacobian.compute(); DenseMatrix64F jacobianMatrix = footJacobian.getJacobianMatrix(); CommonOps.mult(selectionMatrix, jacobianMatrix, linearPartOfJacobian); UnrolledInverseFromMinor.inv3(linearPartOfJacobian, linearJacobianInverse, 1.0); CommonOps.multTransA(linearJacobianInverse, jointTorques, footLinearForce); footForce.setToZero(footJacobian.getJacobianFrame()); footForce.set(footLinearForce.get(0), footLinearForce.get(1), footLinearForce.get(2)); footForce.changeFrame(ReferenceFrame.getWorldFrame()); measuredZForce.set(footForce.getZ() * -1.0); isInContact.set(measuredZForce.getDoubleValue() > zForceThreshold.getDoubleValue()); } }