public Vector3D getCurrentLinkCom() { Vector3D comOffset = new Vector3D(); targetLink.getComOffset(comOffset); return comOffset; }
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"); }
@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 void addForcePoint() { externalForcePoint1 = new ExternalForcePoint("hand1ForcePoint", scsRobotArm.getLink("hand1").getComOffset(), scsRobotArm); externalForcePoint2 = new ExternalForcePoint("hand2ForcePoint", scsRobotArm.getLink("hand2").getComOffset(), scsRobotArm); scsRobotArm.getLink("hand1").getParentJoint().addExternalForcePoint(externalForcePoint1); scsRobotArm.getLink("hand2").getParentJoint().addExternalForcePoint(externalForcePoint2); }
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 void addForcePoint() { externalForcePoint = new ExternalForcePoint("handForcePoint", scsRobotArm.getLink("hand").getComOffset(), scsRobotArm); scsRobotArm.getLink("hand").getParentJoint().addExternalForcePoint(externalForcePoint); }
private void addForcePoint() { externalForcePoint1 = new ExternalForcePoint("hand1ForcePoint", scsRobotArm.getLink("hand1").getComOffset(), scsRobotArm); externalForcePoint2 = new ExternalForcePoint("hand2ForcePoint", scsRobotArm.getLink("hand2").getComOffset(), scsRobotArm); scsRobotArm.getLink("hand1").getParentJoint().addExternalForcePoint(externalForcePoint1); scsRobotArm.getLink("hand2").getParentJoint().addExternalForcePoint(externalForcePoint2); }
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.getComOffset(comOffset);
rootJoint.setLink(link); Vector3D comOffset = new Vector3D(); link.getComOffset(comOffset);
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.getComOffset(comOffset);
link.getComOffset(comOffset);
@Test// timeout=300000 public void testOffsetAtCenterOfMassWithCantileveredBeam() throws UnreasonableAccelerationException { double massOne = 7.21; Robot robot = new Robot("JointWrenchSensorTest"); PinJoint pinJointOne = createPinJointWithHangingMass("pinJointOne", massOne, Axis.Y, robot); Vector3D comOffsetFromJointOne = new Vector3D(); pinJointOne.getLink().getComOffset(comOffsetFromJointOne); JointWrenchSensor jointWrenchSensorOne = new JointWrenchSensor("jointWrenchSensorOne", comOffsetFromJointOne, robot); pinJointOne.addJointWrenchSensor(jointWrenchSensorOne); assertTrue(jointWrenchSensorOne == pinJointOne.getJointWrenchSensor()); robot.addRootJoint(pinJointOne); pinJointOne.setQ(Math.PI/2.0); pinJointOne.setTau(massOne * robot.getGravityZ() * comOffsetFromJointOne.getZ()); robot.doDynamicsAndIntegrate(0.0001); double jointAcceleration = pinJointOne.getQDDYoVariable().getDoubleValue(); assertEquals(0.0, jointAcceleration, 1e-7); Vector3D expectedJointForce = new Vector3D(-massOne * robot.getGravityZ(), 0.0, 0.0); Vector3D expectedJointTorque = new Vector3D(); assertJointWrenchEquals(jointWrenchSensorOne, expectedJointForce, expectedJointTorque); }