/** ID Robot --> SCS Robot Copy the torques from the IDRobot to the SCSRobot. */
public void updateTorquesSCSrobot() // Remember! the body joint is NOT actuated
{
for (OneDoFJoint idJoint : idToSCSLegJointMap.keySet()) {
OneDegreeOfFreedomJoint scsJoint = idToSCSLegJointMap.get(idJoint);
scsJoint.setTau(idJoint.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());
}
}