public RectangleRobot() { super("RectangleRobot"); baseJoint = new FloatingJoint("base", new Vector3D(0.0, 0.0, 0.0), this); Link link = base("base"); baseJoint.setLink(link); this.addRootJoint(baseJoint); }
public MicroStrain3DMRobot() { super("MicroStrain3DMRobot"); ms3DM = new FloatingJoint("ms3DM", new Vector3D(0.0, 0.0, 0.0), this); ms3DM.setLink(MS3DMLink()); this.addRootJoint(ms3DM); this.setGravity(0.0, 0.0, 0.0); addYoVariableRegistry(registry); }
public ContactableSphereRobot(String name, double radius, double mass, AppearanceDefinition color) { super(name); floatingJoint = new FloatingJoint("base", new Vector3D(0.0, 0.0, 0.0), this); sphereLink = ball(radius, mass, color); floatingJoint.setLink(sphereLink); this.addRootJoint(floatingJoint); originalSphere3d = new Sphere3D(radius); currentSphere3d = new Sphere3D(radius); }
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); }
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; }
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); }
public ContactableSelectableBoxRobot(String name, double length, double width, double height, double mass) { super(name); floatingJoint = new FloatingJoint(name + "Base", name, new Vector3D(0.0, 0.0, 0.0), this); linkGraphics = new Graphics3DObject(); linkGraphics.setChangeable(true); boxLink = boxLink(linkGraphics, length, width, height, mass); floatingJoint.setLink(boxLink); this.addRootJoint(floatingJoint); frameBox = new FrameBox3d(ReferenceFrame.getWorldFrame(), length, width, height); Box3D box = frameBox.getBox3d(); boxGraphics = linkGraphics.addCube(box.getSizeX(), box.getSizeY(), box.getSizeZ()); setUpGroundContactPoints(frameBox); unSelect(true); linkGraphics.registerSelectedListener(this); }
public void attachFaceCube() { Link faceLink = new Link("FaceLink"); Graphics3DObject faceGraphics = new Graphics3DObject(); faceGraphics.scale(new Vector3D(3.0, 3.0, 0.1)); faceGraphics.addModelFile("models/GFE/ihmc/face_cube.dae"); faceLink.setLinkGraphics(faceGraphics); FloatingJoint faceJoint = new FloatingJoint("FaceJoint", "faceJoint" , new Vector3D(), this); faceJoint.setRotationAndTranslation(new RigidBodyTransform(new AxisAngle(new Vector3D(0.0, 1.0, 0.0), - Math.PI / 2.0), new Vector3D(1.1, 0.0, 1.3))); faceJoint.setLink(faceLink); faceJoint.setDynamic(false); floatingJoint.addJoint(faceJoint); }
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); }
public void attachFaceCube() { Link faceLink = new Link("FaceLink"); Graphics3DObject faceGraphics = new Graphics3DObject(); faceGraphics.scale(new Vector3d(3.0, 3.0, 0.1)); faceGraphics.addModelFile("models/GFE/ihmc/face_cube.dae"); faceLink.setLinkGraphics(faceGraphics); FloatingJoint faceJoint = new FloatingJoint("FaceJoint", "faceJoint" , new Vector3d(), this); faceJoint.setRotationAndTranslation(new RigidBodyTransform(new AxisAngle4d(new Vector3d(0.0, 1.0, 0.0), - Math.PI / 2.0), new Vector3d(1.1, 0.0, 1.3))); faceJoint.setLink(faceLink); faceJoint.setDynamic(false); floatingJoint.addJoint(faceJoint); }
public void attachFaceCube() { Link faceLink = new Link("FaceLink"); Graphics3DObject faceGraphics = new Graphics3DObject(); faceGraphics.scale(new Vector3d(3.0, 3.0, 0.1)); faceGraphics.addModelFile("models/GFE/ihmc/face_cube.dae"); faceLink.setLinkGraphics(faceGraphics); FloatingJoint faceJoint = new FloatingJoint("FaceJoint", "faceJoint" , new Vector3d(), this); faceJoint.setRotationAndTranslation(new RigidBodyTransform(new AxisAngle4d(new Vector3d(0.0, 1.0, 0.0), - Math.PI / 2.0), new Vector3d(1.1, 0.0, 1.3))); faceJoint.setLink(faceLink); faceJoint.setDynamic(false); floatingJoint.addJoint(faceJoint); }
public ContactableCylinderRobot(String name, RigidBodyTransform rootJointTransform, double radius, double height, double mass, String modelName) { super(name); rootJointTransformToWorld = rootJointTransform; afterRootJointFrame = new ReferenceFrame("rootJointFrame", worldFrame) { @Override protected void updateTransformToParent(RigidBodyTransform transformToParent) { floatingJoint.getTransformToWorld(transformToParent); } }; frameCylinder = new FrameCylinder3d(afterRootJointFrame, height, radius); link = new Link(name + "Link"); link.setMass(mass); link.setComOffset(new Vector3D(0.0, 0.0, height / 3.0)); Matrix3D inertia = RotationalInertiaCalculator.getRotationalInertiaMatrixOfSolidCylinder(mass, radius, height, Axis.Z); link.setMomentOfInertia(inertia); linkGraphics = new Graphics3DObject(); //linkGraphics.addCoordinateSystem(0.2); linkGraphics.addCylinder(frameCylinder.getHeight(), frameCylinder.getRadius(), YoAppearance.Transparent()); if(modelName!=null) linkGraphics.addModelFile(modelName); link.setLinkGraphics(linkGraphics); floatingJoint = new FloatingJoint(name + "Base", name, new Vector3D(), this); floatingJoint.setRotationAndTranslation(rootJointTransform); floatingJoint.setLink(link); this.addRootJoint(floatingJoint); }
@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()); }
rootJoint.setLink(new Link("Dummy")); rootJoint.setDynamic(false); addRootJoint(rootJoint);
rootJoint.setLink(link); robot.addRootJoint(rootJoint);
rootLink.setMomentOfInertia(rootMomentOfInertia); rootJoint.setLink(rootLink); robot.addRootJoint(rootJoint);