private void doRobotDynamics(Robot robot) { try { robot.doDynamicsButDoNotIntegrate(); } catch (UnreasonableAccelerationException e) { throw new RuntimeException(e); } // SimulationConstructionSet scs = new SimulationConstructionSet(robot, false); // scs.disableGUIComponents(); // scs.setRecordDT(scs.getDT()); // Thread simThread = new Thread(scs, "InverseDynamicsCalculatorTest sim thread"); // simThread.start(); // scs.simulate(1); // waitForSimulationToFinish(scs); }
private static void updateRobot(Robot robot) { try { robot.update(); robot.doDynamicsButDoNotIntegrate(); robot.update(); } catch (UnreasonableAccelerationException e) { throw new RuntimeException("UnreasonableAccelerationException"); } }
@Test// timeout=300000 public void testFloatingJointAndPinJointWithMassiveBody() throws UnreasonableAccelerationException { Random random = new Random(1659L); Robot robot = new Robot("r1"); robot.setGravity(0.0); FloatingJoint root1 = new FloatingJoint("root1", new Vector3D(), robot); robot.addRootJoint(root1); Link floatingBody = new Link("floatingBody"); floatingBody.setMass(random.nextDouble()); floatingBody.setComOffset(random.nextDouble(), random.nextDouble(), random.nextDouble()); floatingBody.setMomentOfInertia(getRotationalInertiaMatrixOfSolidEllipsoid(floatingBody.getMass(), random.nextDouble(), random.nextDouble(), random.nextDouble())); root1.setLink(floatingBody); Vector3D offset = EuclidCoreRandomTools.nextVector3D(random); PinJoint pin1 = new PinJoint("pin1", offset, robot, EuclidCoreRandomTools.nextVector3D(random)); pin1.setLink(massiveLink()); root1.addJoint(pin1); pin1.setTau(random.nextDouble()); robot.doDynamicsButDoNotIntegrate(); double scalarInertiaAboutJointAxis = computeScalarInertiaAroundJointAxis(floatingBody, pin1); double torqueFromDynamics = scalarInertiaAboutJointAxis * pin1.getQDDYoVariable().getDoubleValue(); assertEquals(pin1.getTauYoVariable().getDoubleValue(), torqueFromDynamics, pin1.getTauYoVariable().getDoubleValue() * 1e-3); }
@Override public void doScript(double t) { try { robot.doDynamicsButDoNotIntegrate(); } catch (UnreasonableAccelerationException e) { e.printStackTrace(); } calculator.setGravitionalAcceleration(robot.getGravityZ()); getFloatingJointStateFromSCS(); getOneDoFJointStateFromSCS(); multiBodySystem.getRootBody().updateFramesRecursively(); getExternalWrenchesFromSCS(); calculator.compute(); calculator.writeComputedJointAccelerations(multiBodySystem.getJointsToConsider()); if (performAssertions) compareJointAccelerations(1.0e-5 * Math.max(1.0, findAccelerationGreatestMagnitude())); compareJointAccelerations(1.0e-3); }
externalForcePoint.setForce(force); robot.doDynamicsButDoNotIntegrate();
testHelper.setRobotRootJointExternalForcesRandomly(random, maxRootJointExternalForceAndTorque); robot.doDynamicsButDoNotIntegrate(); testHelper.setFullRobotModelStateAndAccelerationToMatchRobot();
testHelper.setRobotTorquesToMatchFullRobotModel(); robot.doDynamicsButDoNotIntegrate();
pinJointOne.addJoint(pinJointTwo); robot.doDynamicsButDoNotIntegrate(); robot.doDynamicsButDoNotIntegrate();
robotB.update(); robotA.doDynamicsButDoNotIntegrate(); robotB.doDynamicsButDoNotIntegrate();
robotB.update(); robotA.doDynamicsButDoNotIntegrate(); robotB.doDynamicsButDoNotIntegrate();
assertEquals(computeScalarInertiaAroundJointAxis(link21, pin1), computeScalarInertiaAroundJointAxis(link22, pin2), epsilonBefore); robot1.doDynamicsButDoNotIntegrate(); robot2.doDynamicsButDoNotIntegrate();
robotB.update(); robotA.doDynamicsButDoNotIntegrate(); robotB.doDynamicsButDoNotIntegrate();
robot.doDynamicsButDoNotIntegrate(); robot.update(); robot.updateVelocities(); robot.doDynamicsButDoNotIntegrate();