@Override public void updateContactPoints() { robot.update(); robot.updateVelocities(); } }
public SDFRobotWriter(Class<? extends Robot> robotClass) throws JAXBException, InstantiationException, IllegalAccessException { System.out.println("Creating SDFRobot for: " + robotClass.getSimpleName()); scsRobot = robotClass.newInstance(); scsRobot.update(); String resourceDirectory = robotClass.getResource(".").getFile(); sdfModelName = scsRobot.getName(); sdfFilePath = resourceDirectory + sdfModelName + ".sdf"; System.out.println("SDF file location: " + sdfFilePath); writeSDFRobotDescriptionFile(); }
@Override public void updateContactPoints() { robot.update(); robot.updateVelocities(); }
public SDFRobotWriter(Class<? extends Robot> robotClass) throws JAXBException, InstantiationException, IllegalAccessException { System.out.println("Creating SDFRobot for: " + robotClass.getSimpleName()); scsRobot = robotClass.newInstance(); scsRobot.update(); String resourceDirectory = robotClass.getResource(".").getFile(); sdfModelName = scsRobot.getName(); sdfFilePath = resourceDirectory + sdfModelName + ".sdf"; System.out.println("SDF file location: " + sdfFilePath); writeSDFRobotDescriptionFile(); }
@Test// timeout=300000 public void testCalculateAngularMomentum() { double epsilon = 1e-7; Robot robot1 = new Robot("r1"); FloatingJoint floatingJoint1 = new FloatingJoint("joint1", new Vector3D(),robot1); robot1.addRootJoint(floatingJoint1); Link onlyLink=new Link("SphericalLink"); onlyLink.setComOffset(new Vector3D(0.0, 0.0, 0.0)); onlyLink.setMass(1.0); onlyLink.setMomentOfInertia(1.0, 1.0, 1.0); floatingJoint1.setLink(onlyLink); floatingJoint1.setPosition(new Point3D(1.0,1.0,1.0)); floatingJoint1.setAngularVelocityInBody(new Vector3D(1.0,0.0,0.0)); floatingJoint1.setVelocity(-1.0, 0.0, 0.0); Vector3D angularMomentumReturned = new Vector3D(); robot1.computeAngularMomentum(angularMomentumReturned); EuclidCoreTestTools.assertTuple3DEquals(new Vector3D(0.0, 0.0, 0.0), angularMomentumReturned, epsilon); robot1.update(); robot1.computeAngularMomentum(angularMomentumReturned); EuclidCoreTestTools.assertTuple3DEquals(new Vector3D(0.0, 0.0, 0.0), angularMomentumReturned, epsilon); robot1.updateVelocities(); robot1.computeAngularMomentum(angularMomentumReturned); //System.out.printf("Momentum <%+3.4f,%+3.4f,%+3.4f>", angularMomentumReturned.x,angularMomentumReturned.y,angularMomentumReturned.z); EuclidCoreTestTools.assertTuple3DEquals(new Vector3D(1.0, -1.0, 1.0), angularMomentumReturned, epsilon); floatingJoint1.setPosition(new Vector3D()); }
private void setPosition(FloatingJoint cubeJoint, double x, double y, double z) { cubeJoint.setPosition(x, y, z); cubeJoint.getRobot().update(); } }
public synchronized void updateRobot() { if (viewportPanel != null) { for (Robot robot : simulation.getRobots()) { robot.update(); } for (GraphicsRobot graphicsRobot : graphicsRobotsToUpdate) { graphicsRobot.update(); } } }
robot.update(); robot.updateVelocities(); robot.doDynamicsButDoNotIntegrate();
public void packRobotComCopSeries(ArrayList<Point3D> outCom, ArrayList<Point3D> outCop) { outCom.clear(); outCop.clear(); for (int i = 0; i < selectedFrames.length; i++) { dataBuffer.setIndexButDoNotNotifySimulationRewoundListeners(selectedFrames[i]); // model predicted CoM robot.update(); //this pull data from dataBuffer magically through YoVariables Point3D modelCoM = new Point3D(); robot.computeCenterOfMass(modelCoM); outCom.add(modelCoM); // sensedCoP Point3D sensedCoP = new Point3D(dataBuffer.getVariable("sensedCoPX").getValueAsDouble(), dataBuffer.getVariable("sensedCoPY").getValueAsDouble(), dataBuffer.getVariable("sensedCoPZ").getValueAsDouble()); outCop.add(sensedCoP); } }
public void testBoxCloseButNoCollisions() { ScsCollisionDetector collisionDetector = createCollisionDetector(); FloatingJoint cubeA = cube(collisionDetector, "A", 10, 0.5, 1.0, 1.5); FloatingJoint cubeB = cube(collisionDetector, "B", 10, 0.75, 1.2, 1.7); double a[] = new double[] { 0.5 + 0.75, 1.0 + 1.2, 1.5 + 1.7 }; // add a bit of separation to ensure they don't collide double tau = 0.001; // should just barely not intersect for (int i = 0; i < 3; i++) { double Tx, Ty, Tz; Tx = Ty = Tz = 0.0; if (i == 0) Tx = a[i] / 2.0 + tau; if (i == 1) Ty = a[i] / 2.0 + tau; if (i == 2) Tz = a[i] / 2.0 + tau; cubeA.setPosition(Tx, Ty, Tz); cubeB.setPosition(-Tx, -Ty, -Tz); cubeA.getRobot().update(); cubeB.getRobot().update(); RigidBodyTransform transformToWorld = new RigidBodyTransform(); cubeA.getTransformToWorld(transformToWorld); CollisionDetectionResult result = new CollisionDetectionResult(); collisionDetector.performCollisionDetection(result); assertEquals(0, result.getNumberOfCollisions()); } }
@Test// timeout = 30000 public void testOneRigidJointWithTranslation() { Robot robot = new Robot("Test"); RigidJoint rigidJointOne = new RigidJoint("rigidJointOne", new Vector3D(), robot); Vector3D translation = new Vector3D(1.1, 2.2, 3.3); rigidJointOne.setRigidTranslation(translation); RigidBodyTransform transform = new RigidBodyTransform(); transform.setTranslation(translation); Link rigidLinkOne = new Link("rigidLinkOne"); double massOne = 1.0; rigidLinkOne.setMassAndRadiiOfGyration(massOne, 0.1, 0.1, 0.1); Vector3D centerOfMassOffset = new Vector3D(0.99, -0.4, 1.1); rigidLinkOne.setComOffset(centerOfMassOffset); rigidJointOne.setLink(rigidLinkOne); robot.addRootJoint(rigidJointOne); robot.update(); Point3D centerOfMass = new Point3D(); double totalMass = robot.computeCenterOfMass(centerOfMass); assertEquals(massOne, totalMass, 1e-7); Point3D expectedCenterOfMass = new Point3D(centerOfMassOffset); expectedCenterOfMass.add(translation); EuclidCoreTestTools.assertTuple3DEquals(expectedCenterOfMass, centerOfMass, 1e-10); }
@Test// timeout = 30000 public void testOneRigidJointWithRotation() { Robot robot = new Robot("Test"); RigidJoint rigidJointOne = new RigidJoint("rigidJointOne", new Vector3D(), robot); RotationMatrix rotation = new RotationMatrix(); Vector3D eulerAngles = new Vector3D(0.3, 0.1, 0.2); rotation.setEuler(eulerAngles); rigidJointOne.setRigidRotation(rotation); Link rigidLinkOne = new Link("rigidLinkOne"); double massOne = 1.0; rigidLinkOne.setMassAndRadiiOfGyration(massOne, 0.1, 0.1, 0.1); Vector3D centerOfMassOffset = new Vector3D(0.3, 0.7, 1.11); rigidLinkOne.setComOffset(centerOfMassOffset); rigidJointOne.setLink(rigidLinkOne); robot.addRootJoint(rigidJointOne); robot.update(); Point3D centerOfMass = new Point3D(); double totalMass = robot.computeCenterOfMass(centerOfMass); assertEquals(massOne, totalMass, 1e-7); Point3D expectedCenterOfMass = new Point3D(centerOfMassOffset); rotation.transform(expectedCenterOfMass); EuclidCoreTestTools.assertTuple3DEquals(expectedCenterOfMass, centerOfMass, 1e-10); }
@Test// timeout = 30000 public void testOneRigidJointWithRotationAndTranslation() { Robot robot = new Robot("Test"); RigidJoint rigidJointOne = new RigidJoint("rigidJointOne", new Vector3D(), robot); RotationMatrix rotation = new RotationMatrix(); Vector3D eulerAngles = new Vector3D(0.3, 0.1, 0.2); rotation.setEuler(eulerAngles); rigidJointOne.setRigidRotation(rotation); Vector3D translation = new Vector3D(0.3, -0.99, 1.11); rigidJointOne.setRigidTranslation(translation); RigidBodyTransform transform = new RigidBodyTransform(); transform.setTranslation(translation); transform.setRotation(rotation); Link rigidLinkOne = new Link("rigidLinkOne"); double massOne = 1.0; rigidLinkOne.setMassAndRadiiOfGyration(massOne, 0.1, 0.1, 0.1); Vector3D centerOfMassOffset = new Vector3D(0.3, 0.7, 1.11); rigidLinkOne.setComOffset(centerOfMassOffset); rigidJointOne.setLink(rigidLinkOne); robot.addRootJoint(rigidJointOne); robot.update(); Point3D centerOfMass = new Point3D(); double totalMass = robot.computeCenterOfMass(centerOfMass); assertEquals(massOne, totalMass, 1e-7); Point3D expectedCenterOfMass = new Point3D(centerOfMassOffset); transform.transform(expectedCenterOfMass); EuclidCoreTestTools.assertTuple3DEquals(expectedCenterOfMass, centerOfMass, 1e-10); }