public void reset() { for (int i = 0; i < listOfBodiesWithExternalWrenches.size(); i++) { Wrench externalWrench = externalWrenches.get(listOfBodiesWithExternalWrenches.get(i)); externalWrench.setToZero(externalWrench.getBodyFrame(), externalWrench.getExpressedInFrame()); } }
public void setExternalWrenchToCompensateFor(RigidBody rigidBody, Wrench wrench) { boolean containsRigidBody = externalWrenchesToCompensateFor.get(rigidBody) != null; if (!containsRigidBody) { externalWrenches.put(rigidBody, new Wrench(rigidBody.getBodyFixedFrame(), rigidBody.getBodyFixedFrame())); externalWrenchesToCompensateFor.put(rigidBody, new Wrench(rigidBody.getBodyFixedFrame(), rigidBody.getBodyFixedFrame())); externalWrenchesToCompensateForList.add(externalWrenchesToCompensateFor.get(rigidBody)); rigidBodiesWithWrenchToCompensateFor.add(rigidBody); } ReferenceFrame bodyFixedFrame = rigidBody.getBodyFixedFrame(); wrench.getBodyFrame().checkReferenceFrameMatch(bodyFixedFrame); wrench.getExpressedInFrame().checkReferenceFrameMatch(bodyFixedFrame); externalWrenchesToCompensateFor.get(rigidBody).set(wrench); }
public void setWrench(Wrench newWrench) { measurementFrame.checkReferenceFrameMatch(newWrench.getExpressedInFrame()); measurementFrame.checkReferenceFrameMatch(newWrench.getBodyFrame()); newWrench.getMatrix(wrench); }
public void setWrench(Wrench jointWrench) { successorWrench.getBodyFrame().checkReferenceFrameMatch(jointWrench.getBodyFrame()); successorWrench.getExpressedInFrame().checkReferenceFrameMatch(jointWrench.getExpressedInFrame()); successorWrench.setAngularPartY(jointWrench.getAngularPartY()); successorWrench.setLinearPartX(jointWrench.getLinearPartX()); successorWrench.setLinearPartZ(jointWrench.getLinearPartZ()); }
@Override public void setTorqueFromWrench(Wrench jointWrench) { unitSuccessorTwist.getBodyFrame().checkReferenceFrameMatch(jointWrench.getBodyFrame()); unitSuccessorTwist.getExpressedInFrame().checkReferenceFrameMatch(jointWrench.getExpressedInFrame()); this.tau = unitSuccessorTwist.dot(jointWrench); // cheating a little bit; tau = J^T * wrench. J maps joint velocities to joint twists. // the unit twist is actually exactly the same as J, except that its entries have different dimensions. // we disregard dimensions and just use .dot(.) for efficiency }
tempWrench.changeFrame(tempWrench.getBodyFrame()); tempPoint.setToZero(wrench.getBodyFrame()); tempPoint.changeFrame(ReferenceFrame.getWorldFrame()); pointOfApplication.set(tempPoint);
@Override public void setTorqueFromWrench(Wrench jointWrench) { jointWrench.getBodyFrame().checkReferenceFrameMatch(successor.getBodyFixedFrame()); jointWrench.getExpressedInFrame().checkReferenceFrameMatch(jointTorque.getReferenceFrame()); jointTorque.set(jointWrench.getAngularPart()); }
public void getWrench(Wrench wrenchToPack) { wrenchToPack.setToZero(successorWrench.getBodyFrame(), successorWrench.getExpressedInFrame()); wrenchToPack.setAngularPartY(successorWrench.getAngularPartY()); wrenchToPack.setLinearPartX(successorWrench.getLinearPartX()); wrenchToPack.setLinearPartZ(successorWrench.getLinearPartZ()); }
/** * Takes the dot product of this twist and a wrench, resulting in the (reference frame independent) instantaneous power. * @param wrench a wrench that * 1) has an 'onWhat' reference frame that is the same as this twist's 'bodyFrame' reference frame. * 2) is expressed in the same reference frame as this twist * @return the instantaneous power associated with this twist and the wrench */ public double dot(Wrench wrench) { this.bodyFrame.checkReferenceFrameMatch(wrench.getBodyFrame()); this.expressedInFrame.checkReferenceFrameMatch(wrench.getExpressedInFrame()); double power = this.angularPart.dot(wrench.getAngularPart()) + this.linearPart.dot(wrench.getLinearPart()); return power; }
@Override public void setTorqueFromWrench(Wrench jointWrench) { jointWrench.getBodyFrame().checkReferenceFrameMatch(successor.getBodyFixedFrame()); jointWrench.setToZero(successor.getBodyFixedFrame(), successorWrench.getExpressedInFrame()); jointWrench.setAngularPartY(successorWrench.getAngularPartY()); jointWrench.setLinearPartX(successorWrench.getLinearPartX()); jointWrench.setLinearPartZ(successorWrench.getLinearPartZ()); jointWrench.changeFrame(successor.getBodyFixedFrame()); }
public void computeDynamicWrenchInBodyCoordinates(SpatialAccelerationVector acceleration, Twist twist, Wrench dynamicWrenchToPack) // TODO: write test { checkExpressedInBodyFixedFrame(); checkIsCrossPartZero(); // otherwise this operation would be a lot less efficient acceleration.getBodyFrame().checkReferenceFrameMatch(this.bodyFrame); acceleration.getBaseFrame().checkIsWorldFrame(); acceleration.getExpressedInFrame().checkReferenceFrameMatch(this.expressedInframe); twist.getBodyFrame().checkReferenceFrameMatch(this.bodyFrame); twist.getBaseFrame().checkIsWorldFrame(); twist.getExpressedInFrame().checkReferenceFrameMatch(this.expressedInframe); dynamicWrenchToPack.getBodyFrame().checkReferenceFrameMatch(this.bodyFrame); dynamicWrenchToPack.getExpressedInFrame().checkReferenceFrameMatch(this.expressedInframe); massMomentOfInertiaPart.transform(acceleration.getAngularPart(), tempVector); // J * omegad dynamicWrenchToPack.setAngularPart(tempVector); // [J * omegad; 0] massMomentOfInertiaPart.transform(twist.getAngularPart(), tempVector); // J * omega tempVector.cross(twist.getAngularPart(), tempVector); // omega x J * omega dynamicWrenchToPack.addAngularPart(tempVector); // [J * omegad + omega x J * omega; 0] tempVector.set(acceleration.getLinearPart()); // vd tempVector.scale(mass); // m * vd dynamicWrenchToPack.setLinearPart(tempVector); // [J * omegad + omega x J * omega; m * vd] tempVector.set(twist.getLinearPart()); // v tempVector.scale(mass); // m * v tempVector.cross(twist.getAngularPart(), tempVector); // omega x m * v dynamicWrenchToPack.addLinearPart(tempVector); // [J * omegad + omega x J * omega; m * vd + omega x m * v] }