private PinJoint createPinJointWithHangingMass(String name, double mass, Axis axis, Robot robot) { PinJoint pinJoint = new PinJoint(name, new Vector3D(), robot, axis); Vector3D comOffset = new Vector3D(0.0, 0.0, -1.0); Link linkOne = new Link("link"); linkOne.setMass(mass); linkOne.setComOffset(comOffset); linkOne.setMassAndRadiiOfGyration(mass, 0.1, 0.1, 0.1); pinJoint.setLink(linkOne); return pinJoint; }
public DoublePendulum() { super("DoublePendulum"); j1 = new PinJoint("j1", new Vector3D(0, 0, 2), this, Axis.X); Link l1 = new Link("l1"); l1.setComOffset(0, 0, 0.5); l1.setMassAndRadiiOfGyration(1.0, 0.05, 0.05, 0.3); l1.addEllipsoidFromMassProperties(YoAppearance.Pink()); j1.setLink(l1); j2 = new PinJoint("j2", new Vector3D(0.0, 0.0, 1.0), this, Axis.X); Link l2 = new Link("l2"); l2.setComOffset(0, 0, 0.5); l2.setMassAndRadiiOfGyration(1.0, 0.05, 0.05, 0.3); l2.addEllipsoidFromMassProperties(YoAppearance.Purple()); j2.setLink(l2); j1.addJoint(j2); addRootJoint(j1); }
public FloatingObjectBoxRobot(String imageResourcePath) { super(FloatingObjectBoxRobot.class.getSimpleName()); qrCodeJoint = new FloatingJoint("object", "object", new Vector3D(), this); Link qrCodeLink = new Link("object"); qrCodeLink.setMassAndRadiiOfGyration(1.0, 0.1, 0.1, 0.1); Graphics3DObject qrCodeLinkGraphics = new Graphics3DObject(); // qrCodeLinkGraphics.addCoordinateSystem(2.0); double cubeLength = 1.0; qrCodeLinkGraphics.translate(0.0, 0.0, -0.99 * cubeLength); AppearanceDefinition cubeAppearance = YoAppearance.Texture(imageResourcePath); qrCodeLinkGraphics.addCube(cubeLength * 0.98, cubeLength * 1.01, cubeLength * 0.98, YoAppearance.Yellow()); boolean[] textureFaces = new boolean[] { true, true, false, false, false, false }; qrCodeLinkGraphics.translate(0.0, 0.0, -0.01 * cubeLength); qrCodeLinkGraphics.addCube(cubeLength, cubeLength, cubeLength, cubeAppearance, textureFaces); qrCodeLink.setLinkGraphics(qrCodeLinkGraphics); qrCodeJoint.setLink(qrCodeLink); addRootJoint(qrCodeJoint); setGravity(0.0); }
@Test// timeout = 30000 public void testOneRigidJoint() { Robot robot = new Robot("Test"); RigidJoint rigidJointOne = new RigidJoint("rigidJointOne", new Vector3D(), robot); Link rigidLinkOne = new Link("rigidLinkOne"); double massOne = 1.0; rigidLinkOne.setMassAndRadiiOfGyration(massOne, 0.1, 0.1, 0.1); Vector3D centerOfMassOffset = new Vector3D(1.1, 2.2, 3.3); rigidLinkOne.setComOffset(centerOfMassOffset); rigidJointOne.setLink(rigidLinkOne); robot.addRootJoint(rigidJointOne); Point3D centerOfMass = new Point3D(); double totalMass = robot.computeCenterOfMass(centerOfMass); assertEquals(massOne, totalMass, 1e-7); EuclidCoreTestTools.assertTuple3DEquals(centerOfMassOffset, centerOfMass, 1e-10); }
@Test// timeout = 30000 public void testDummyOneDegreeOfFreedomJoint() { Robot robot = new Robot("testDummyOneDegreeOfFreedomJoint"); Vector3D jointAxis = new Vector3D(0.0, 1.0, 0.0); DummyOneDegreeOfFreedomJoint jointOne = new DummyOneDegreeOfFreedomJoint("jointOne", new Vector3D(), robot, jointAxis); Link linkOne = new Link("linkOne"); linkOne.setMassAndRadiiOfGyration(1.0, 0.1, 0.1, 0.1); Graphics3DObject linkOneGraphics = new Graphics3DObject(); linkOneGraphics.addCylinder(0.1, 0.01); linkOne.setLinkGraphics(linkOneGraphics); jointOne.setLink(linkOne); robot.addRootJoint(jointOne); if (keepSCSUp) { SimulationConstructionSetParameters parameters = SimulationTestingParameters.createFromSystemProperties(); SimulationConstructionSet scs = new SimulationConstructionSet(robot, parameters); scs.startOnAThread(); ThreadTools.sleepForever(); } } }
public FloatingFiducialBoxRobot(Fiducial fiducial) { super("FloatingFiducialBoxRobot"); qrCodeJoint = new FloatingJoint("qrCode", "qrCode", new Vector3D(), this); Link qrCodeLink = new Link("qrCode"); qrCodeLink.setMassAndRadiiOfGyration(1.0, 0.1, 0.1, 0.1); Graphics3DObject qrCodeLinkGraphics = new Graphics3DObject(); // qrCodeLinkGraphics.addCoordinateSystem(2.0); double cubeLength = 1.0; qrCodeLinkGraphics.translate(0.0, 0.0, -0.99 * cubeLength); AppearanceDefinition cubeAppearance = YoAppearance.Texture(fiducial.getPathString()); qrCodeLinkGraphics.addCube(cubeLength * 0.98, cubeLength * 1.01, cubeLength * 0.98, YoAppearance.Yellow()); boolean[] textureFaces = new boolean[] { true, true, false, false, false, false }; qrCodeLinkGraphics.translate(0.0, 0.0, -0.01 * cubeLength); qrCodeLinkGraphics.addCube(cubeLength, cubeLength, cubeLength, cubeAppearance, textureFaces); qrCodeLink.setLinkGraphics(qrCodeLinkGraphics); qrCodeJoint.setLink(qrCodeLink); addRootJoint(qrCodeJoint); setGravity(0.0); }
@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 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); }
bodyLink.setMassAndRadiiOfGyration(10.0, 0.1, 0.2, 0.3); armLink1.setMassAndRadiiOfGyration(0.3, 0.1, 0.1, 0.1); armLink1.setComOffset(new Vector3d(0.0, 0.0, 0.5)); armLink2.setMassAndRadiiOfGyration(0.2, 0.1, 0.1, 0.1); armLink2.setComOffset(new Vector3d(0.2, 0.0, 0.0));
@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); }
linkOneA.setMassAndRadiiOfGyration(massOne, radiiOfGyrationOne); linkOneB.setMassAndRadiiOfGyration(massOne, radiiOfGyrationOne); linkTwoB.setMassAndRadiiOfGyration(massTwo, radiiOfGyrationTwo);
cylinderLink.setMassAndRadiiOfGyration(DEFAULT_MASS, 1.0, 1.0, 1.0); cylinderLink.setComOffset(new Vector3D());
link.setMassAndRadiiOfGyration(1.0, radius, radius, radius); Graphics3DObject linkGraphics = new Graphics3DObject(); link.setLinkGraphics(linkGraphics);
linkOneB.setMassAndRadiiOfGyration(massOne, radiiOfGyrationOne);
linkOneB.setMassAndRadiiOfGyration(massOne, radiiOfGyrationOne);