private Link link1() { Link ret = new Link("link1"); ret.setMass(1.0); ret.setComOffset(2.0, 0.0, 0.0); ret.setMomentOfInertia(0.0, 3.0, 0.0); return ret; }
private Link base(String name) { Link ret = new Link(name); ret.setMass(0.1); Graphics3DObject linkGraphics = new Graphics3DObject(); linkGraphics.addCoordinateSystem(0.5); ret.setLinkGraphics(linkGraphics); return ret; }
private void printLinkInformation(Link link, StringBuffer buffer) { double mass = link.getMass(); Vector3D comOffset = new Vector3D(); link.getComOffset(comOffset); Matrix3D momentOfInertia = new Matrix3D(); link.getMomentOfInertia(momentOfInertia); buffer.append("Mass = " + mass + "\n"); buffer.append("comOffset = " + comOffset + "\n"); buffer.append("momentOfInertia = \n" + momentOfInertia + "\n"); }
private Link massiveLink() { Link ret = new Link("massiveLink"); ret.setMass(1e12); ret.setMomentOfInertia(1e8, 1e8, 1e8); return ret; }
Link linkOneA = new Link("linkOneA"); Link linkOneB = new Link("linkOneB"); linkOneA.setMassAndRadiiOfGyration(massOne, radiiOfGyrationOne); linkOneB.setMassAndRadiiOfGyration(massOne, radiiOfGyrationOne); linkOneA.setComOffset(centerOfMassOffsetOne); linkOneB.setComOffset(centerOfMassOffsetOne); rigidJointB.setRigidRotation(rigidJointRotation); rigidJointB.setRigidTranslation(rigidJointOffset); Link rigidLinkB = new Link("rigidLinkB"); rigidJointB.setLink(rigidLinkB); Link linkTwoA = new Link("linkTwoA"); Link linkTwoB = new Link("linkTwoB"); linkTwoA.setMass(massTwo); linkTwoB.setMassAndRadiiOfGyration(massTwo, radiiOfGyrationTwo); linkTwoB.getMomentOfInertia(momentOfInertiaTwo); Matrix3D rotatedMomentOfInertiaTwo = new Matrix3D(momentOfInertiaTwo); rigidJointRotation.transform(rotatedMomentOfInertiaTwo); linkTwoA.setMomentOfInertia(rotatedMomentOfInertiaTwo); linkTwoA.setComOffset(rotatedCenterOfMassOffsetTwo); linkTwoB.setComOffset(centerOfMassOffsetTwo);
private Link exampleArcTorusShape() { Link ret = new Link("exampleArcTorusShape"); Graphics3DObject linkGraphics = new Graphics3DObject(); // ArcTorus linkGraphics.translate(OFFSET, 0.0, 0.0); linkGraphics.addCoordinateSystem(COORD_LENGTH); linkGraphics.addArcTorus(ARC_TORUS_START_ANG, ARC_TORUS_END_ANG, ARC_TORUS_MAJ_RAD, ARC_TORUS_MIN_RAD, YoAppearance.DarkRed()); ret.setLinkGraphics(linkGraphics); return ret; }
Link link1 = new Link("link1"); link1.setMass(mass1); link1.setMomentOfInertia(momentOfInertia1); link1.setComOffset(comOffset1); joint1.setLink(link1); Link link2 = new Link("link2"); link2.setMass(mass2); link2.setMomentOfInertia(momentOfInertia2); link2.setComOffset(comOffset2); joint2.setLink(link2); link.getComOffset(comOffset); link.getMomentOfInertia(momentOfInertia); Link expectedLink = new Link("expectedLink"); expectedLink.setMass(totalMass); expectedLink.setComOffset(expectedCoMOffset); expectedLink.setMomentOfInertia(expectedMomentOfInertia); expectedJoint.setLink(expectedLink); expectedRobot.addRootJoint(expectedJoint);
@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(); } } }
cylinderLink = new Link(name + "Link"); cylinderLink.setMassAndRadiiOfGyration(DEFAULT_MASS, 1.0, 1.0, 1.0); cylinderLink.setComOffset(new Vector3D()); cylinderLink.setLinkGraphics(linkGraphics);
@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); }
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 UniversalJointRobot(String name, double initialQFirstJoint, double initialQdFirstJoint, double initialQSecondJoint, double initialQdSecondJoint) { super(name); universalJoint = new UniversalJoint(name + "Joint1", name + "Joint2", new Vector3D(), this, Axis.X, Axis.Y); Link link = new Link(name + "Link"); link.setMass(mass); link.setComOffset(0.0, 0.0, -length); universalJoint.setLink(link); universalJoint.getFirstJoint().setInitialState(initialQFirstJoint, initialQdFirstJoint); universalJoint.getSecondJoint().setInitialState(initialQSecondJoint, initialQdSecondJoint); addRootJoint(universalJoint); }
link.getMomentOfInertia(momentOfInertia); double mass = link.getMass(); link.getComOffset(comOffset); new RigidBody(link.getName(), currentIDJoint, momentOfInertia, mass, comOffset); currentIDJoint.setJointLimitUpper(currentJoint.getJointUpperLimit()); new RigidBody(link.getName(), currentIDJoint, momentOfInertia, mass, comOffset); currentIDJoint.setJointLimitUpper(currentJoint.getJointUpperLimit()); new RigidBody(link.getName(), currentIDJoint, momentOfInertia, mass, comOffset);
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); }
private Inertial createSDFInertial(Link scsLink) { Inertial sdfInertial = new Inertial(); sdfInertial.setInertia(createSDFInertia(scsLink)); sdfInertial.setMass(String.valueOf(scsLink.getMass())); RigidBodyTransform comOffsetInWorld = new RigidBodyTransform(); scsLink.getParentJoint().getTransformToWorld(comOffsetInWorld); Vector3D com = new Vector3D(); scsLink.getComOffset(com); RigidBodyTransform comOffset = TransformTools.createTranslationTransform(com); comOffsetInWorld.multiply(comOffset); sdfInertial.setPose(getPoseFromTransform3D(comOffsetInWorld)); return sdfInertial; }
@Override public void process(double[] inParameter, double[] outError) { if (lockComY) { Vector3D lastCom = new Vector3D(); targetLink.getComOffset(lastCom); targetLink.setComOffset(inParameter[0], lastCom.getY(), inParameter[2]); } else { targetLink.setComOffset(new Vector3D(inParameter)); } ArrayList<Point3D> com = new ArrayList<>(selectedFrames.length); ArrayList<Point3D> cop = new ArrayList<>(selectedFrames.length); packRobotComCopSeries(com, cop); for (int i = 0; i < com.size(); i++) { outError[2 * i] = cop.get(i).getX() - com.get(i).getX(); outError[2 * i + 1] = cop.get(i).getY() - com.get(i).getY(); } }
private void addForcePoint() { externalForcePoint = new ExternalForcePoint("handForcePoint", scsRobotArm.getLink("hand").getComOffset(), scsRobotArm); scsRobotArm.getLink("hand").getParentJoint().addExternalForcePoint(externalForcePoint); }
private List<SDFVisual> createSDFVisual(Link scsLink) { if (scsLink.getName().contains("body")) System.out.println(); List<SDFVisual> sdfVisuals = new ArrayList<>(); ArrayList<Graphics3DPrimitiveInstruction> graphics3dInstructions = scsLink.getLinkGraphics().getGraphics3DInstructions(); RigidBodyTransform parentJointTransformToWorld = new RigidBodyTransform(); Joint parentJoint = scsLink.getParentJoint(); if (scsRobot.getRootJoints().contains(parentJoint)) parentJoint.getTransformToWorld(parentJointTransformToWorld); SDFVisual sdfVisual = createSDFVisual(parentJointTransformToWorld, graphics3dInstructions); if (sdfVisual != null) sdfVisuals.add(sdfVisual); return sdfVisuals; }
public void addJointAxis(FloatingRootJointRobot valkyrieRobot) { ArrayList<OneDegreeOfFreedomJoint> joints = new ArrayList<>(Arrays.asList(valkyrieRobot.getOneDegreeOfFreedomJoints())); for (OneDegreeOfFreedomJoint joint : joints) { Graphics3DObject linkGraphics = new Graphics3DObject(); linkGraphics.addCoordinateSystem(0.5); linkGraphics.combine(joint.getLink().getLinkGraphics()); joint.getLink().setLinkGraphics(linkGraphics); } }
@Override public void setMass(double mass) { link.setMass(mass); }