@Override public double getJointAccelerationProcessedOutput(OneDoFJoint oneDoFJoint) { return oneDoFJoint.getQdd(); }
@Override public double getJointAccelerationRawOutput(OneDoFJoint oneDoFJoint) { return oneDoFJoint.getQdd(); }
@Override public double getJointAccelerationProcessedOutput(OneDoFJoint oneDoFJoint) { for(int i = 0; i < sensorOneDoFJoints.length; i++) { if(sensorOneDoFJoints[i].getName() == oneDoFJoint.getName()) { return sensorOneDoFJoints[i].getQdd(); } } return 0.0; }
public static void integrateAccelerations(Iterable<? extends OneDoFJoint> joints, double dt) { for (OneDoFJoint joint : joints) { joint.setQd(joint.getQd() + joint.getQdd() * dt); } }
@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.setQ(revoluteJoint.getQ()); pinJoint.setQd(revoluteJoint.getQd()); pinJoint.setQdd(revoluteJoint.getQdd()); } } }
@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()); }
public void updateRobotConfigurationBasedOnFullRobotModel() { for (ImmutablePair<OneDegreeOfFreedomJoint, OneDoFJoint> jointPair : revoluteJoints) { OneDegreeOfFreedomJoint pinJoint = jointPair.getLeft(); OneDoFJoint revoluteJoint = jointPair.getRight(); pinJoint.setQ(revoluteJoint.getQ()); pinJoint.setQd(revoluteJoint.getQd()); pinJoint.setQdd(revoluteJoint.getQdd()); } FloatingJoint floatingJoint = rootJointPair.getLeft(); FloatingInverseDynamicsJoint sixDoFJoint = rootJointPair.getRight(); RigidBodyTransform transform = sixDoFJoint.getJointTransform3D(); floatingJoint.setRotationAndTranslation(transform); }
public void updateRobotConfigurationBasedOnFullRobotModel() { for (ImmutablePair<OneDegreeOfFreedomJoint, OneDoFJoint> jointPair : revoluteJoints) { OneDegreeOfFreedomJoint pinJoint = jointPair.getLeft(); OneDoFJoint revoluteJoint = jointPair.getRight(); pinJoint.setQ(revoluteJoint.getQ()); pinJoint.setQd(revoluteJoint.getQd()); pinJoint.setQdd(revoluteJoint.getQdd()); } FloatingJoint floatingJoint = rootJointPair.getLeft(); FloatingInverseDynamicsJoint sixDoFJoint = rootJointPair.getRight(); RigidBodyTransform transform = sixDoFJoint.getJointTransform3D(); floatingJoint.setRotationAndTranslation(transform); } }