@Override public double getJointVelocityProcessedOutput(OneDoFJoint oneDoFJoint) { return oneDoFJoint.getQd(); }
public static void integrateAccelerations(Iterable<? extends OneDoFJoint> joints, double dt) { for (OneDoFJoint joint : joints) { joint.setQd(joint.getQd() + joint.getQdd() * dt); } }
public double getValue() { return joint.getQd(); } }
@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 double getJointVelocityRawOutput(OneDoFJoint oneDoFJoint) { return oneDoFJoint.getQd(); }
@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 get(double[] buffer, int offset) { buffer[offset + 0] = joint.getQ(); buffer[offset + 1] = joint.getQd(); }
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 get(double[] buffer, int offset) { buffer[offset + 0] = joint.getQ(); buffer[offset + 1] = joint.getQd(); }
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 setJointState(ArrayList<OneDoFJoint> newJointData) { if (newJointData.size() != jointAngles.length) throw new RuntimeException("Array size does not match"); for (int i = 0; i < jointAngles.length; i++) { jointAngles[i] = (float) newJointData.get(i).getQ(); jointVelocities[i] = (float) newJointData.get(i).getQd(); jointTorques[i] = (float) newJointData.get(i).getTauMeasured(); } }
@Override public void initialize() { initialize(joint.getQ(), joint.getQd()); }
public static void integrateVelocity(OneDoFJoint joint, double dt) { double oldQ = joint.getQ(); double deltaQ = joint.getQd() * dt; double newQ = oldQ + deltaQ; joint.setQ(newQ); }
@Override public double getJointVelocityProcessedOutput(OneDoFJoint oneDoFJoint) { for(int i = 0; i < sensorOneDoFJoints.length; i++) { if(sensorOneDoFJoints[i].getName() == oneDoFJoint.getName()) { return sensorOneDoFJoints[i].getQd(); } } return 0.0; }
@Override public void initialize() { initialize(joint.getQ(), joint.getQd()); }
private void controlJoint(StepprJoint joint, double qDesired, double qdDesired) { OneDoFJoint oneDoFJoint = joints.get(joint); PDController controller = controllers.get(joint); double tau = controller.compute(oneDoFJoint.getQ(), qDesired, oneDoFJoint.getQd(), qdDesired); oneDoFJoint.setTau(tau); }
@Override public void doControl(long timestamp) { for(int i = 0; i < controllers.size(); i++) { OneDoFJoint joint = joints.get(i); PDController controller = controllers.get(i); double tauFF = tauFFs.get(i).getDoubleValue(); double q_d = desiredPositions.get(i).getDoubleValue(); double qd_d = desiredVelocities.get(i).getDoubleValue(); double tau = controller.compute(joint.getQ(), q_d, joint.getQd(), qd_d) + tauFF; joint.setTau(tau); joint.setKd(damping.get(i).getDoubleValue()); } }
@Override public void doControl(long timestamp) { for(int i = 0; i < controllers.size(); i++) { OneDoFJoint joint = joints.get(i); PDController controller = controllers.get(i); double tauFF = tauFFs.get(i).getDoubleValue(); double q_d = desiredPositions.get(i).getDoubleValue(); double qd_d = desiredVelocities.get(i).getDoubleValue(); double tau = controller.compute(joint.getQ(), q_d, joint.getQd(), qd_d) + tauFF; joint.setTau(tau); joint.setKd(damping.get(i).getDoubleValue()); } }
@Override public void updateAnkleState(OneDoFJoint ankleX, OneDoFJoint ankleY) { q_AnkleX = ankleX.getQ(); q_AnkleY = ankleY.getQ(); measuredJointVelocities.set(0,ankleX.getQd()); measuredJointVelocities.set(1,ankleY.getQd()); desiredJointTorque.set(0,ankleX.getTau()); desiredJointTorque.set(1,ankleY.getTau()); updateAnkleStateFromJointAngles(q_AnkleX, q_AnkleY); computeDesiredAcutatorTau(); }
@Override public void compute() { if (!isEnabled.getBooleanValue()) return; qCurrent.set(joint.getQ()); qDCurrent.set(joint.getQd()); qError.set(qDesired.getDoubleValue() - qCurrent.getDoubleValue()); qDError.set(qDDesired.getDoubleValue() - qDCurrent.getDoubleValue()); double qdd_fb = kp.getDoubleValue() * qError.getDoubleValue() + kd.getDoubleValue() * qDError.getDoubleValue(); qdd_fb = MathTools.clipToMinMax(qdd_fb, maxFeedbackAcceleration.getDoubleValue()); qDDFeedback.set(qdd_fb); qDDFeedbackRateLimited.update(); qDDDesired.set(qDDFeedforward.getDoubleValue() + qDDFeedbackRateLimited.getDoubleValue()); output.setOneDoFJointDesiredAcceleration(0, qDDDesired.getDoubleValue()); }