@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 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 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 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); }
floatingJoint1.setLink(randomBodyNoYCoMOffset(random)); PinJoint pin1 = new PinJoint("pin1", offset, robot1, Axis.Y); floatingJoint1.addJoint(pin1); pin1.setLink(randomBodyNoYCoMOffset(random));
pinJoint1.addIMUMount(imuMount1); rootJoint.addJoint(pinJoint1);
link.setLinkGraphics(linkGraphics); lidarJoint.setLink(link); rootJoint.addJoint(lidarJoint);