@Override public void assertStateIsCloseToClosedFormCalculation(double epsilon) throws AssertionError { double q = pinJoint.getQ(); double qd = pinJoint.getQD(); double qdd = pinJoint.getQDD(); double qddLagrangian = (mass * getGravityZ() * (0.5 * length) * Math.sin(q) - damping * qd) / (Ixx + mass * MathTools.square(0.5 * length)); if(Math.abs(qdd - qddLagrangian) > epsilon) { throw new AssertionError("Joint accelerations from simulation and lagrangian don't match. At t=" + getTime() + ", simulated joint acceleration = " + qdd + ", lagrangian acceleration = " + qddLagrangian); } } }
@Override public void assertStateIsCloseToClosedFormCalculation(double epsilon) { double qx = universalJoint.getFirstJoint().getQ(); double qddx = universalJoint.getFirstJoint().getQDD(); double qy = universalJoint.getSecondJoint().getQ(); double qddy = universalJoint.getSecondJoint().getQDD(); double g = Math.abs(gravityZ.getDoubleValue()); if(Math.abs(Math.cos(qy)) < 1e-4) return; double qddxLagrangian = - (g * Math.sin(qx)) / (length * Math.cos(qy)); double qddyLagrangian = - (g * Math.cos(qx) * Math.sin(qy)) / length; if(Math.abs(qddxLagrangian - qddx) > epsilon || Math.abs(qddyLagrangian - qddy) > epsilon) { throw new AssertionError("Joint accelerations from simulation and lagrangian don't match. " + "\nAt t=" + getTime() + "\nSimulated joint accelerations: (" + qddx + ", " + qddy + ")" + "\nLagrangian accelerations: (" + qddxLagrangian + ", " + qddyLagrangian + ")"); } } }
private void getOneDoFJointStateFromSCS() { for (OneDoFJointBasics joint : allOneDoFJoints) { PinJoint scsJoint = (PinJoint) robot.getJoint(joint.getName()); joint.setQ(scsJoint.getQ()); joint.setQd(scsJoint.getQD()); double tau = scsJoint.getTau(); if (scsJoint.tauDamping != null) tau += scsJoint.tauDamping.getValue(); if (scsJoint.tauJointLimit != null) tau += scsJoint.tauJointLimit.getValue(); if (scsJoint.tauVelocityLimit != null) tau += scsJoint.tauVelocityLimit.getValue(); joint.setTau(tau); } }
double xdd = sliderJoint.getQDD(); double theta = pinJoint.getQ(); double thetaD = pinJoint.getQD(); double thetaDD = pinJoint.getQDD();