public void setRobotTorquesToMatchOtherRobot(FloatingRootJointRobot otherRobot) { ArrayList<OneDegreeOfFreedomJoint> oneDegreeOfFreedomJoints = new ArrayList<OneDegreeOfFreedomJoint>(); robot.getAllOneDegreeOfFreedomJoints(oneDegreeOfFreedomJoints); ArrayList<OneDegreeOfFreedomJoint> otherOneDegreeOfFreedomJoints = new ArrayList<OneDegreeOfFreedomJoint>(); otherRobot.getAllOneDegreeOfFreedomJoints(otherOneDegreeOfFreedomJoints); for (int i = 0; i < oneDegreeOfFreedomJoints.size(); i++) { OneDegreeOfFreedomJoint oneDegreeOfFreedomJoint = oneDegreeOfFreedomJoints.get(i); OneDegreeOfFreedomJoint otherOneDegreeOfFreedomJoint = otherOneDegreeOfFreedomJoints.get(i); if (!oneDegreeOfFreedomJoint.getName().equals(otherOneDegreeOfFreedomJoint.getName())) { throw new RuntimeException(); } oneDegreeOfFreedomJoint.setTau(otherOneDegreeOfFreedomJoint.getTauYoVariable().getDoubleValue()); } }
public void startComputation() { jointTorque.setValue(joint.getTauYoVariable().getDoubleValue()); corrupt(jointTorque); jointTorqueOutputPort.setData(jointTorque); }
public void setRobotTorquesToMatchFullRobotModelButCheckAgainstOtherRobot(FloatingRootJointRobot otherRobot, double epsilon) { ArrayList<OneDegreeOfFreedomJoint> oneDegreeOfFreedomJoints = new ArrayList<OneDegreeOfFreedomJoint>(); robot.getAllOneDegreeOfFreedomJoints(oneDegreeOfFreedomJoints); ArrayList<OneDegreeOfFreedomJoint> otherOneDegreeOfFreedomJoints = new ArrayList<OneDegreeOfFreedomJoint>(); otherRobot.getAllOneDegreeOfFreedomJoints(otherOneDegreeOfFreedomJoints); for (int i = 0; i < oneDegreeOfFreedomJoints.size(); i++) { OneDegreeOfFreedomJoint oneDegreeOfFreedomJoint = oneDegreeOfFreedomJoints.get(i); OneDegreeOfFreedomJoint otherOneDegreeOfFreedomJoint = otherOneDegreeOfFreedomJoints.get(i); OneDoFJointBasics oneDoFJoint = fullRobotModel.getOneDoFJointByName(oneDegreeOfFreedomJoint.getName()); double inverseDynamicsTorque = oneDoFJoint.getTau(); oneDegreeOfFreedomJoint.setTau(inverseDynamicsTorque); YoDouble computedJointTorque = computedJointTorques.get(oneDoFJoint); computedJointTorque.set(inverseDynamicsTorque); double otherRobotJointTorque = otherOneDegreeOfFreedomJoint.getTauYoVariable().getDoubleValue(); YoDouble differenceJointTorque = differenceJointTorques.get(oneDoFJoint); differenceJointTorque.set(otherRobotJointTorque - inverseDynamicsTorque); if (Math.abs(differenceJointTorque.getDoubleValue()) > epsilon) { System.err.println("Torques don't match. oneDegreeOfFreedomJoint = " + oneDegreeOfFreedomJoint.getName() + ", otherRobotJointTorque = " + otherRobotJointTorque + ", inverseDynamicsTorque = " + inverseDynamicsTorque); } } }
private void createAndAddOneDoFPositionAndVelocitySensors(SCSToInverseDynamicsJointMap scsToInverseDynamicsJointMap) { ArrayList<OneDegreeOfFreedomJoint> oneDegreeOfFreedomJoints = new ArrayList<OneDegreeOfFreedomJoint>(scsToInverseDynamicsJointMap.getSCSOneDegreeOfFreedomJoints()); for (OneDegreeOfFreedomJoint oneDegreeOfFreedomJoint : oneDegreeOfFreedomJoints) { OneDoFJointBasics oneDoFJoint = scsToInverseDynamicsJointMap.getInverseDynamicsOneDoFJoint(oneDegreeOfFreedomJoint); simulatedSensorHolderAndReader.addJointPositionSensorPort(oneDoFJoint, oneDegreeOfFreedomJoint.getQYoVariable()); simulatedSensorHolderAndReader.addJointVelocitySensorPort(oneDoFJoint, oneDegreeOfFreedomJoint.getQDYoVariable()); simulatedSensorHolderAndReader.addJointTorqueSensorPort(oneDoFJoint, oneDegreeOfFreedomJoint.getTauYoVariable()); } }
@ContinuousIntegrationTest(estimatedDuration = 21.5) @Test(timeout = 64580) public void testDoNothingBahviourState() throws SimulationExceededMaximumTimeException { testState(HighLevelControllerName.DO_NOTHING_BEHAVIOR); OneDegreeOfFreedomJoint[] oneDofJoints = drcBehaviorTestHelper.getRobot().getOneDegreeOfFreedomJoints(); for (OneDegreeOfFreedomJoint joint : oneDofJoints) { String jointName = joint.getName(); double tau = joint.getTauYoVariable().getDoubleValue(); if (!jointName.contains("hokuyo")) { assertTrue(joint.getName() + " tau : " + tau, tau == 0.0); } } }
double simulatedRobotTorque = oneDegreeOfFreedomJoint.getTauYoVariable().getDoubleValue();