currentJoint.getOffset(jointOffset);
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; }
currentJoint.getOffset(jointOffset);