public void cross(FrameTuple<?, ?> frameTuple1, FrameTuple<?, ?> frameTuple2) { checkReferenceFrameMatch(frameTuple1); checkReferenceFrameMatch(frameTuple2); cross(this.tuple, frameTuple1.tuple, frameTuple2.tuple); }
public void cross(FrameTuple<?, ?> frameTuple1) { checkReferenceFrameMatch(frameTuple1); cross(this.tuple, this.tuple, frameTuple1.tuple); }
public double angle(FrameVector frameVector) { checkReferenceFrameMatch(frameVector); return this.tuple.angle(frameVector.tuple); }
public double dot(FrameVector frameVector) { checkReferenceFrameMatch(frameVector); return this.tuple.dot(frameVector.tuple); }
public PrismaticJointReferenceFrame(String frameName, ReferenceFrame parentFrame, FrameVector axis) { super(frameName, parentFrame); axis.checkReferenceFrameMatch(parentFrame); this.axis = axis.getVectorCopy(); }
public RevoluteJointReferenceFrame(String frameName, ReferenceFrame parentFrame, FrameVector axis) { super(frameName, parentFrame); axis.checkReferenceFrameMatch(parentFrame); this.axis = axis.getVectorCopy(); }
/** * Sets this spatial motion vector based */ public void set(ReferenceFrame bodyFrame, ReferenceFrame baseFrame, ReferenceFrame expressedInFrame, FrameVector linearPart, FrameVector angularPart) { linearPart.checkReferenceFrameMatch(expressedInFrame); angularPart.checkReferenceFrameMatch(expressedInFrame); this.bodyFrame = bodyFrame; this.baseFrame = baseFrame; this.expressedInFrame = expressedInFrame; linearPart.get(this.linearPart); angularPart.get(this.angularPart); }
public boolean isEpsilonParallel(FrameVector frameVector, double epsilonAngle) { checkReferenceFrameMatch(frameVector); double angleMinusZeroToPi = Math.abs(AngleTools.trimAngleMinusPiToPi(this.angle(frameVector))); double errorFromParallel = Math.min(angleMinusZeroToPi, Math.PI - angleMinusZeroToPi); return errorFromParallel < epsilonAngle; }
public void set(FramePoint desiredPosition, FrameVector desiredLinearVelocity, FrameVector feedForwardLinearAcceleration) { desiredPosition.checkReferenceFrameMatch(worldFrame); desiredLinearVelocity.checkReferenceFrameMatch(worldFrame); feedForwardLinearAcceleration.checkReferenceFrameMatch(worldFrame); desiredPosition.get(desiredPositionInWorld); desiredLinearVelocity.get(desiredLinearVelocityInWorld); feedForwardLinearAcceleration.get(feedForwardLinearAccelerationInWorld); }
public void set(FrameOrientation desiredOrientation, FrameVector desiredAngularVelocity, FrameVector feedForwardAngularAcceleration) { desiredOrientation.checkReferenceFrameMatch(worldFrame); desiredAngularVelocity.checkReferenceFrameMatch(worldFrame); feedForwardAngularAcceleration.checkReferenceFrameMatch(worldFrame); desiredOrientation.getQuaternion(desiredOrientationInWorld); desiredAngularVelocity.get(desiredAngularVelocityInWorld); feedForwardAngularAcceleration.get(feedForwardAngularAccelerationInWorld); }
public void set(FramePoint desiredPosition, FrameVector desiredLinearVelocity, FrameVector feedForwardLinearAcceleration) { desiredPosition.checkReferenceFrameMatch(worldFrame); desiredLinearVelocity.checkReferenceFrameMatch(worldFrame); feedForwardLinearAcceleration.checkReferenceFrameMatch(worldFrame); desiredPosition.get(desiredPositionInWorld); desiredLinearVelocity.get(desiredLinearVelocityInWorld); feedForwardLinearAcceleration.get(feedForwardLinearAccelerationInWorld); }
public void set(FrameOrientation desiredOrientation, FrameVector desiredAngularVelocity, FrameVector feedForwardAngularAcceleration) { desiredOrientation.checkReferenceFrameMatch(worldFrame); desiredAngularVelocity.checkReferenceFrameMatch(worldFrame); feedForwardAngularAcceleration.checkReferenceFrameMatch(worldFrame); desiredOrientation.getQuaternion(desiredOrientationInWorld); desiredAngularVelocity.get(desiredAngularVelocityInWorld); feedForwardAngularAcceleration.get(feedForwardAngularAccelerationInWorld); }
private void updateInputs() { // TODO: IMU is not always at body root joint, make this more general for arbitrary IMU position FrameVector bodyAcceleration = processedIMUSensors.getAcceleration(bodyIMUIndex); bodyAcceleration.checkReferenceFrameMatch(world); for (Direction direction : Direction.values()) { inputs.get(direction).set(0, bodyAcceleration.get(direction)); } }
private void computeRootJointAcceleration(FloatingInverseDynamicsJoint rootJoint, SpatialAccelerationVector rootJointAcceleration, FrameVector rootJointAngularAcceleration, FrameVector rootJointLinearAcceleration) { rootJointAngularAcceleration.checkReferenceFrameMatch(rootJoint.getFrameAfterJoint()); rootJointLinearAcceleration.checkReferenceFrameMatch(rootJoint.getFrameAfterJoint()); rootJointAcceleration.set(rootJoint.getFrameAfterJoint(), rootJoint.getFrameBeforeJoint(), rootJoint.getFrameAfterJoint(), rootJointLinearAcceleration.getVector(), rootJointAngularAcceleration.getVector()); }
public void setIncludingFrame(FrameVector force, FramePoint pointOfApplication) { force.checkReferenceFrameMatch(pointOfApplication); expressedInFrame = force.getReferenceFrame(); force.get(linearPart); pointOfApplication.get(tempVector); angularPart.cross(tempVector, linearPart); }
public FramePlane3d(FrameVector normal, FramePoint point) { normal.checkReferenceFrameMatch(point); this.referenceFrame = normal.getReferenceFrame(); this.plane3d = new Plane3d(point.getPoint(), normal.getVector()); }
private void computeRootJointTwist(FloatingInverseDynamicsJoint rootJoint, Twist rootJointTwistToPack, FrameVector rootJointAngularVelocity, FrameVector rootJointLinearVelocity) { rootJointAngularVelocity.checkReferenceFrameMatch(rootJoint.getFrameAfterJoint()); rootJointLinearVelocity.checkReferenceFrameMatch(rootJoint.getFrameAfterJoint()); rootJointTwistToPack.set(rootJoint.getFrameAfterJoint(), rootJoint.getFrameBeforeJoint(), rootJoint.getFrameAfterJoint(), rootJointLinearVelocity.getVector(), rootJointAngularVelocity.getVector()); }
public void setIncludingFrame(FrameVector force, FrameVector moment, FramePoint pointOfApplication) { force.checkReferenceFrameMatch(pointOfApplication); expressedInFrame = force.getReferenceFrame(); force.get(linearPart); pointOfApplication.get(tempVector); angularPart.cross(tempVector, linearPart); angularPart.add(moment.getVector()); }
public RevoluteJoint(String name, RigidBody predecessor, ReferenceFrame beforeJointFrame, FrameVector jointAxis) { super(name, predecessor, beforeJointFrame, new RevoluteJointReferenceFrame(name, beforeJointFrame, jointAxis)); jointAxis.checkReferenceFrameMatch(beforeJointFrame); this.jointAxis = new FrameVector(jointAxis); unitJointTwist = new Twist(afterJointFrame, beforeJointFrame, afterJointFrame, new Vector3d(), jointAxis.getVector()); }
private void computeRootJointTwistLinearPart(FloatingInverseDynamicsJoint rootJoint, Twist rootJointTwistToPack, FrameVector rootJointLinearVelocity) { rootJoint.getJointTwist(tempRootJointTwistExisting); tempRootJointTwistExisting.checkReferenceFramesMatch(rootJoint.getFrameAfterJoint(), rootJoint.getFrameBeforeJoint(), rootJoint.getFrameAfterJoint()); tempRootJointTwistExisting.getAngularPart(tempRootJointTwistExistingAngularPart); rootJointLinearVelocity.checkReferenceFrameMatch(rootJoint.getFrameAfterJoint()); rootJointTwistToPack.set(rootJoint.getFrameAfterJoint(), rootJoint.getFrameBeforeJoint(), rootJoint.getFrameAfterJoint(), rootJointLinearVelocity.getVector(), tempRootJointTwistExistingAngularPart.getVector()); }