private Robot createRobot() { Robot robot = new Robot("RandomRobot"); PinJoint pinJoint = new PinJoint("TestPinJoint", new Vector3D(), robot, new Vector3D(0, 0, 1)); robot.addRootJoint(pinJoint); return robot; } }
@Test// timeout=300000 public void testGetAndSetParentJoint() { PinJoint joint = new PinJoint("joint", new Vector3D(0.0, 0.0, 0.0), robot, Axis.X); robot.addRootJoint(joint); kinematicPoint.setParentJoint(joint); assertTrue(joint == kinematicPoint.getParentJoint()); }
private Robot createSimpleRobotOne(String name) { Robot robot = new Robot(name); PinJoint joint1 = new PinJoint("joint", new Vector3D(0.0, 0.0, 0.0), robot, Axis.Y); Link link1 = link1(); joint1.setLink(link1); robot.addRootJoint(joint1); joint1.setInitialState(0.11, 0.22); joint1.setTau(33.3); return robot; }
private Robot createSimpleRobotTwo(String name) { Robot robot = new Robot(name); PinJoint joint2 = new PinJoint("joint", new Vector3D(0.0, 0.0, 0.0), robot, Axis.Y); Link link2 = link2(); joint2.setLink(link2); robot.addRootJoint(joint2); joint2.setInitialState(0.11, 0.22); joint2.setTau(33.3); return robot; }
private Robot createSimpleRobot() { Robot robot0 = new Robot("robot"); FloatingJoint floatingJoint0 = new FloatingJoint("floatingJoint", new Vector3D(), robot0); Link link0 = new Link("body"); link0.setMass(1.0); link0.setMomentOfInertia(0.1, 0.1, 0.1); floatingJoint0.setLink(link0); robot0.addRootJoint(floatingJoint0); return robot0; }
@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); }
currentJoint.setInitialState(jointPosition, jointVelocity); if (previousJoint == null) robot.addRootJoint(currentJoint); else previousJoint.addJoint(currentJoint);
public LaunchedBall(String name, Robot robot, double collisionDistance, double density) { super(name, name, new Vector3D(), robot); setDynamic(false); setPositionAndVelocity(1000.0, 1000.0, -1000.0, 0.0, 0.0, 0.0); // Hide them away at the start. Link link = new Link(name); Graphics3DObject linkGraphics = new Graphics3DObject(); linkGraphics.setChangeable(true); linkGraphicsScale = linkGraphics.scale(1.0); linkGraphics.addSphere(0.1); link.setLinkGraphics(linkGraphics); setLink(link); robot.addRootJoint(this); this.collisionDistance = collisionDistance; this.density = density; }
public FloatingJoint cube(ScsCollisionDetector collisionDetector, String name, double mass, RigidBodyTransform shapeToLink, double radiusX, double radiusY, double radiusZ, int collisionGroup, int collisionMask) { Robot robot = new Robot("null"); FloatingJoint joint = new FloatingJoint("cube", new Vector3D(), robot); Link link = new Link(name); // link.setMass(mass); // link.setMomentOfInertia(0.1 * mass, 0.1 * mass, 0.1 * mass); // link.enableCollisions(10,null); CollisionShapeFactory factory = collisionDetector.getShapeFactory(); factory.setMargin(0.0000002); CollisionShapeDescription<?> shapeDesc = factory.createBox(radiusX, radiusY, radiusZ); factory.addShape(link, shapeToLink, shapeDesc, false, collisionGroup, collisionMask); joint.setLink(link); robot.addRootJoint(joint); return joint; }
@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(); } } }
@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=300000 public void testOffsetAtCenterOfMassWithCantileveredBeam() throws UnreasonableAccelerationException { double massOne = 7.21; Robot robot = new Robot("JointWrenchSensorTest"); PinJoint pinJointOne = createPinJointWithHangingMass("pinJointOne", massOne, Axis.Y, robot); Vector3D comOffsetFromJointOne = new Vector3D(); pinJointOne.getLink().getComOffset(comOffsetFromJointOne); JointWrenchSensor jointWrenchSensorOne = new JointWrenchSensor("jointWrenchSensorOne", comOffsetFromJointOne, robot); pinJointOne.addJointWrenchSensor(jointWrenchSensorOne); assertTrue(jointWrenchSensorOne == pinJointOne.getJointWrenchSensor()); robot.addRootJoint(pinJointOne); pinJointOne.setQ(Math.PI/2.0); pinJointOne.setTau(massOne * robot.getGravityZ() * comOffsetFromJointOne.getZ()); robot.doDynamicsAndIntegrate(0.0001); double jointAcceleration = pinJointOne.getQDDYoVariable().getDoubleValue(); assertEquals(0.0, jointAcceleration, 1e-7); Vector3D expectedJointForce = new Vector3D(-massOne * robot.getGravityZ(), 0.0, 0.0); Vector3D expectedJointTorque = new Vector3D(); assertJointWrenchEquals(jointWrenchSensorOne, expectedJointForce, expectedJointTorque); }
@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); }
private void createFloatingRobot() { Robot floatingRobot = new Robot("floatingRobot"); Vector3D position = new Vector3D(0.0, 0.02, 1.1); double length = 0.01; floatingRobot.setGravity(0.0, 0.0, 0.0); horizontalJoint = new SliderJoint("y", position, floatingRobot, Axis.Y); floatingRobot.addRootJoint(horizontalJoint); Link linkHorizontal = new Link("linkHorizontal"); linkHorizontal.setMass(0.5); linkHorizontal.setComOffset(length / 2.0, 0.0, 0.0); linkHorizontal.setMomentOfInertia(0.0, 0.01, 0.0); Graphics3DObject linkHorizontalGraphics = new Graphics3DObject(); linkHorizontalGraphics.addCylinder(length * 10, 0.005, YoAppearance.Orange()); linkHorizontal.setLinkGraphics(linkHorizontalGraphics); horizontalJoint.setLink(linkHorizontal); verticalJoint = new SliderJoint("z", new Vector3D(0.0, 0.0, 0.0), floatingRobot, Axis.Z); Link linkVertical = new Link("linkVertical"); linkVertical.setMass(0.5); linkVertical.setComOffset(length / 2.0, 0.0, 0.0); linkVertical.setMomentOfInertia(0.0, 0.01, 0.0); Graphics3DObject linkVerticalGraphics = new Graphics3DObject(); linkVerticalGraphics.addCylinder(length, 0.005, YoAppearance.Blue()); linkVertical.setLinkGraphics(linkVerticalGraphics); verticalJoint.setLink(linkVertical); horizontalJoint.addJoint(verticalJoint); createFloatingRobotController(); robots[0] = floatingRobot; robots[0].setController(floatingRobotController); }
pinJointOne.addJointWrenchSensor(jointWrenchSensorOne); assertTrue(jointWrenchSensorOne == pinJointOne.getJointWrenchSensor()); robot.addRootJoint(pinJointOne);
@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()); }
pinJointOne.addJointWrenchSensor(jointWrenchSensorOne); assertTrue(jointWrenchSensorOne == pinJointOne.getJointWrenchSensor()); robot.addRootJoint(pinJointOne);
robot.addRootJoint(rootJoint);