@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(); } }
data.simulatedJoint.setQdDesired(data.jointData.getDesiredVelocity());
protected void write() { for (int i = 0; i < revoluteJoints.size(); i++) { OneDegreeOfFreedomJoint pinJoint = revoluteJoints.first(i); JointDesiredOutputReadOnly data = revoluteJoints.second(i); if(data.hasDesiredTorque()) { pinJoint.setTau(data.getDesiredTorque()); } if (data.hasStiffness()) { pinJoint.setKp(data.getStiffness()); } if (data.hasDamping()) { pinJoint.setKd(data.getDamping()); } if (data.hasDesiredPosition()) { pinJoint.setqDesired(data.getDesiredPosition()); } if (data.hasDesiredVelocity()) { pinJoint.setQdDesired(data.getDesiredVelocity()); } } }