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 static Link nextLink(long seed) { Random random = new Random(seed); Link link = new Link("randomLink" + random.nextInt()); link.setMass(random.nextDouble()); link.setComOffset(RandomNumbers.nextDouble(random, 1.0), RandomNumbers.nextDouble(random, 1.0), RandomNumbers.nextDouble(random, 1.0)); link.setMomentOfInertia(RobotTest.getRotationalInertiaMatrixOfSolidEllipsoid(link.getMass(), random.nextDouble(), random.nextDouble(), random.nextDouble())); return link; }
private static Link randomBody(Random random) { Link ret = new Link("floatingBody"); ret.setMass(random.nextDouble()); ret.setComOffset(random.nextDouble(), random.nextDouble(), random.nextDouble()); ret.setMomentOfInertia(getRotationalInertiaMatrixOfSolidEllipsoid(ret.getMass(), random.nextDouble(), random.nextDouble(), random.nextDouble())); Graphics3DObject linkGraphics = new Graphics3DObject(); linkGraphics.addCoordinateSystem(COORDINATE_SYSTEM_LENGTH); createInertiaEllipsoid(ret, linkGraphics, YoAppearance.Orange()); ret.setLinkGraphics(linkGraphics); return ret; }
private static Link randomBodyNoYCoMOffset(Random random) { Link ret = new Link("floatingBody"); ret.setMass(random.nextDouble()); ret.setComOffset(random.nextDouble(), 0.0, random.nextDouble()); ret.setMomentOfInertia(getRotationalInertiaMatrixOfSolidEllipsoid(ret.getMass(), random.nextDouble(), random.nextDouble(), random.nextDouble())); Graphics3DObject linkGraphics = new Graphics3DObject(); linkGraphics.addCoordinateSystem(COORDINATE_SYSTEM_LENGTH); createInertiaEllipsoid(ret, linkGraphics, YoAppearance.Orange()); ret.setLinkGraphics(linkGraphics); return ret; }
System.out.println("target link " + targetLink.getName() + "mass " + targetLink.getMass() + "kg, com " + getCurrentLinkCom());
private Link createRandomLink(String linkName, boolean useZeroCoMOffset) { Link link = new Link(linkName); link.setMomentOfInertia(random.nextDouble(), random.nextDouble(), random.nextDouble()); link.setMass(random.nextDouble()); Vector3D comOffset = useZeroCoMOffset ? new Vector3D() : new Vector3D(random.nextDouble(), random.nextDouble(), random.nextDouble()); link.setComOffset(comOffset); Graphics3DObject linkGraphics = new Graphics3DObject(); Matrix3D momentOfInertia = new Matrix3D(); link.getMomentOfInertia(momentOfInertia); double mass = link.getMass(); linkGraphics.createInertiaEllipsoid(momentOfInertia, comOffset, mass, YoAppearance.Red()); link.setLinkGraphics(linkGraphics); return link; }
public static void createInertiaEllipsoid(Link ret, Graphics3DObject linkGraphics, AppearanceDefinition appearance) { Matrix3D momentOfInertia = new Matrix3D(); ret.getMomentOfInertia(momentOfInertia); Vector3D comOffset = new Vector3D(); ret.getComOffset(comOffset); double mass = ret.getMass(); linkGraphics.createInertiaEllipsoid(momentOfInertia, comOffset, mass, appearance); }
private RigidBodyBasics copyLinkAsRigidBody(Link link, JointBasics currentInverseDynamicsJoint, String bodyName) { Vector3D comOffset = new Vector3D(); link.getComOffset(comOffset); Matrix3D momentOfInertia = new Matrix3D(); link.getMomentOfInertia(momentOfInertia); return new RigidBody(bodyName, currentInverseDynamicsJoint, momentOfInertia, link.getMass(), comOffset); }
private static RigidBodyBasics copyLinkAsRigidBody(Link link, JointBasics currentInverseDynamicsJoint, String bodyName) { Vector3D comOffset = new Vector3D(); link.getComOffset(comOffset); Matrix3D momentOfInertia = new Matrix3D(); link.getMomentOfInertia(momentOfInertia); return new RigidBody(bodyName, currentInverseDynamicsJoint, momentOfInertia, link.getMass(), comOffset); }
private static Link link11(Random random, double l1, double r1) { Link ret = new Link("link11"); ret.setMass(random.nextDouble()); ret.setComOffset(0.0, 0.0, -l1 / 2.0); ret.setMomentOfInertia(getRotationalInertiaMatrixOfSolidEllipsoid(ret.getMass(), r1, r1, l1 / 2.0)); Graphics3DObject linkGraphics = new Graphics3DObject(); linkGraphics.addCoordinateSystem(COORDINATE_SYSTEM_LENGTH); createInertiaEllipsoid(ret, linkGraphics, YoAppearance.Red()); ret.setLinkGraphics(linkGraphics); return ret; }
private static Link link21(Random random, double l2, double r2) { Link ret = new Link("link2"); ret.setMass(random.nextDouble()); ret.setComOffset(0.0, 0.0, l2 / 2.0); // ret.setComOffset(0.0, 0.0, 0.0); ret.setMomentOfInertia(getRotationalInertiaMatrixOfSolidEllipsoid(ret.getMass(), r2, r2, l2 / 2.0)); Graphics3DObject linkGraphics = new Graphics3DObject(); linkGraphics.addCoordinateSystem(COORDINATE_SYSTEM_LENGTH); createInertiaEllipsoid(ret, linkGraphics, YoAppearance.Orange()); ret.setLinkGraphics(linkGraphics); return ret; }
@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); }
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; }
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; }
private static Link link22(Link link21) { Link ret = new Link("link22"); ret.setComOffset(link21.getComOffset()); ret.setMass(link21.getMass()); Matrix3D link2moi = new Matrix3D(); link21.getMomentOfInertia(link2moi); ret.setMomentOfInertia(link2moi); Graphics3DObject linkGraphics = new Graphics3DObject(); linkGraphics.addCoordinateSystem(COORDINATE_SYSTEM_LENGTH); createInertiaEllipsoid(ret, linkGraphics, YoAppearance.Aqua()); ret.setLinkGraphics(linkGraphics); return ret; }
link.getMomentOfInertia(momentOfInertia); double mass = link.getMass();
private static Link link12(Link link11, PinJoint pin1) { Link ret = new Link("link12"); Vector3D comOffset12 = new Vector3D(link11.getComOffset()); Vector3D offset = new Vector3D(); pin1.getOffset(offset); comOffset12.sub(offset); ret.setComOffset(comOffset12); ret.setMass(link11.getMass()); Matrix3D link1moi = new Matrix3D(); link11.getMomentOfInertia(link1moi); ret.setMomentOfInertia(link1moi); Graphics3DObject linkGraphics = new Graphics3DObject(); linkGraphics.addCoordinateSystem(COORDINATE_SYSTEM_LENGTH); createInertiaEllipsoid(ret, linkGraphics, YoAppearance.Blue()); ret.setLinkGraphics(linkGraphics); return ret; }
private static double computeScalarInertiaAroundJointAxis(Link link, PinJoint pinJoint) { Matrix3D momentOfInertia = new Matrix3D(); link.getMomentOfInertia(momentOfInertia); Vector3D jointAxis = new Vector3D(); pinJoint.getJointAxis(jointAxis); Vector3D temp1 = new Vector3D(jointAxis); momentOfInertia.transform(temp1); double scalarInertiaAboutCoM = jointAxis.dot(temp1); // jointAxis^T * momentOfInertia * jointAxis double mass = link.getMass(); Vector3D offsetToCoM = new Vector3D(link.getComOffset()); Vector3D offset = new Vector3D(); pinJoint.getOffset(offset); offsetToCoM.sub(offset); // c - p Vector3D temp3 = new Vector3D(jointAxis); temp3.scale(offsetToCoM.dot(jointAxis)); // ((c - p) . a) * a Vector3D comToJointAxis = new Vector3D(); comToJointAxis.sub(offsetToCoM, temp3); // (c - p) - ((c - p) . a) * a double distanceToJointAxis = comToJointAxis.length(); double scalarInertiaAboutJointAxis = scalarInertiaAboutCoM + mass * distanceToJointAxis * distanceToJointAxis; return scalarInertiaAboutJointAxis; }
Double mass = link.getMass(); addHeaderEntry(configDataSheet, column, "Mass"); addNumberToSheet(configDataSheet, column++, row, mass);
link.getMomentOfInertia(momentOfInertia); double mass = link.getMass();