@Override public void writeAfterController(long timestamp) { for (int i = 0; i < revoluteJoints.size(); i++) { ImmutablePair<OneDegreeOfFreedomJoint, OneDoFJoint> jointPair = revoluteJoints.get(i); OneDegreeOfFreedomJoint pinJoint = jointPair.getLeft(); OneDoFJoint revoluteJoint = jointPair.getRight(); double tau = revoluteJoint.getTau(); DoubleYoVariable rawJointTorque = rawJointTorques.get(revoluteJoint); DelayedDoubleYoVariable delayedJointTorque = delayedJointTorques.get(revoluteJoint); if (rawJointTorque != null) { rawJointTorque.set(tau); delayedJointTorque.update(); tau = delayedJointTorque.getDoubleValue(); } pinJoint.setTau(tau); pinJoint.setKp(revoluteJoint.getKp()); pinJoint.setKd(revoluteJoint.getKd()); pinJoint.setqDesired(revoluteJoint.getqDesired()); pinJoint.setQdDesired(revoluteJoint.getQdDesired()); } for (int i = 0; i < rawOutputWriters.size(); i++) { rawOutputWriters.get(i).write(); } }
yoTauInertiaViz.get(joint).set(motorInertiaTorque); double kd = (wholeBodyControlJoint.getDamping()+masterMotorDamping.getDoubleValue()*yoMotorDamping.get(joint).getDoubleValue()) * controlRatio + standPrepJoint.getKd() * (1.0 - controlRatio); double tau = (wholeBodyControlJoint.getDesiredTorque()+ motorInertiaTorque+desiredQddFeedForwardTorque) * controlRatio + standPrepJoint.getTau() * (1.0 - controlRatio);
yoTauInertiaViz.get(joint).set(motorInertiaTorque); double kd = (wholeBodyControlJoint.getDamping()+masterMotorDamping.getDoubleValue()*yoMotorDamping.get(joint).getDoubleValue()) * controlRatio + standPrepJoint.getKd() * (1.0 - controlRatio); double tau = (wholeBodyControlJoint.getDesiredTorque()+ motorInertiaTorque+desiredQddFeedForwardTorque) * controlRatio + standPrepJoint.getTau() * (1.0 - controlRatio);
jointCommand.setDamping(oneDoFJoint.getKd());
jointCommand.setDamping(oneDoFJoint.getKd());