public void updateRobotConfigurationBasedOnFullRobotModel() {
for (int i = 0; i < revoluteJoints.size(); i++) {
ImmutablePair<OneDegreeOfFreedomJoint, OneDoFJoint> jointPair = revoluteJoints.get(i);
OneDegreeOfFreedomJoint pinJoint = jointPair.getLeft();
OneDoFJoint revoluteJoint = jointPair.getRight();
pinJoint.setQ(revoluteJoint.getQ());
}
FloatingJoint floatingJoint = rootJointPair.getLeft();
FloatingInverseDynamicsJoint sixDoFJoint = rootJointPair.getRight();
RigidBodyTransform transform = sixDoFJoint.getJointTransform3D();
floatingJoint.setRotationAndTranslation(transform);
}
public double getValue() {
return joint.getQ();
}