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 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; }
private SDFInertia createSDFInertia(Link scsLink) { SDFInertia sdfInertia = new SDFInertia(); Matrix3d scsInertia = new Matrix3d(); scsLink.getMomentOfInertia(scsInertia); sdfInertia.setIxx(String.valueOf(scsInertia.getM00())); sdfInertia.setIyy(String.valueOf(scsInertia.getM11())); sdfInertia.setIzz(String.valueOf(scsInertia.getM22())); sdfInertia.setIxy(String.valueOf(scsInertia.getM01())); sdfInertia.setIxz(String.valueOf(scsInertia.getM02())); sdfInertia.setIyz(String.valueOf(scsInertia.getM12())); return sdfInertia; }
private SDFInertia createSDFInertia(Link scsLink) { SDFInertia sdfInertia = new SDFInertia(); Matrix3D scsInertia = new Matrix3D(); scsLink.getMomentOfInertia(scsInertia); sdfInertia.setIxx(String.valueOf(scsInertia.getM00())); sdfInertia.setIyy(String.valueOf(scsInertia.getM11())); sdfInertia.setIzz(String.valueOf(scsInertia.getM22())); sdfInertia.setIxy(String.valueOf(scsInertia.getM01())); sdfInertia.setIxz(String.valueOf(scsInertia.getM02())); sdfInertia.setIyz(String.valueOf(scsInertia.getM12())); return sdfInertia; }
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); }
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 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);
floatingBody.getMomentOfInertia(momentOfInertia);
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; }
link.getMomentOfInertia(momentOfInertia);
link.getMomentOfInertia(momentOfInertia);
linkOneB.getMomentOfInertia(momentOfInertiaOne); Matrix3D rotatedMomentOfInertiaOne = new Matrix3D(momentOfInertiaOne); rigidJointRotation.transform(rotatedMomentOfInertiaOne);
linkOneB.getMomentOfInertia(momentOfInertiaOne); Matrix3D rotatedMomentOfInertiaOne = new Matrix3D(momentOfInertiaOne); rigidJointRotation.transform(rotatedMomentOfInertiaOne);
link.getMomentOfInertia(momentOfInertia);
linkTwoB.getMomentOfInertia(momentOfInertiaTwo); Matrix3D rotatedMomentOfInertiaTwo = new Matrix3D(momentOfInertiaTwo); rigidJointRotation.transform(rotatedMomentOfInertiaTwo);