public double dot(FrameVector vector) { return this.getFrameTuple().dot(vector); }
public static void checkJointAxesAreParallel(FrameVector masterAxis, FrameVector jointBAxis, FrameVector jointCAxis, FrameVector jointDAxis) { masterAxis.changeFrame(worldFrame); jointBAxis.changeFrame(worldFrame); jointCAxis.changeFrame(worldFrame); jointDAxis.changeFrame(worldFrame); if(DEBUG) { System.out.println("\nDebugging axis dot products: \nmaster x B = " + masterAxis.dot(jointBAxis) + "\nmaster x C = " + masterAxis.dot(jointCAxis) + "\nmaster x D = " + masterAxis.dot(jointDAxis) ); } // Both the exact same axis and a flipped axis are valid (eg: y and -y). So as long as the absolute value of the dot product is 1, the axis are parallel. double epsilon = 1.0e-9; boolean isJointBParallel = MathTools.epsilonEquals(Math.abs(masterAxis.dot(jointBAxis)), 1.0, epsilon); boolean isJointCParallel = MathTools.epsilonEquals(Math.abs(masterAxis.dot(jointCAxis)), 1.0, epsilon); boolean isJointDParallel = MathTools.epsilonEquals(Math.abs(masterAxis.dot(jointDAxis)), 1.0, epsilon); if (!isJointBParallel || !isJointCParallel || !isJointDParallel) { throw new RuntimeException("All joints in the four bar must rotate around the same axis!"); } }
public int compare(FramePoint o1, FramePoint o2) { differenceVector.setIncludingFrame(o1); differenceVector.sub(o2); double dotProduct = searchDirection.dot(differenceVector); return Double.compare(dotProduct, 0.0); } }
@Override public void doControl() { forceSensorData.getWrench(tempWrench); for(int i = 0; i < yoTorqueInJoints.size(); i++) { ImmutablePair<FrameVector, DoubleYoVariable> pair = yoTorqueInJoints.get(i); FrameVector jointAxis = pair.getLeft(); DoubleYoVariable torqueAboutJointAxis = pair.getRight(); tempWrench.changeFrame(jointAxis.getReferenceFrame()); tempFrameVector.setToZero(tempWrench.getExpressedInFrame()); tempWrench.getAngularPart(tempFrameVector); torqueAboutJointAxis.set(-tempFrameVector.dot(jointAxis)); } }
@Override public void doControl() { forceSensorData.getWrench(tempWrench); for(int i = 0; i < yoTorqueInJoints.size(); i++) { ImmutablePair<FrameVector, DoubleYoVariable> pair = yoTorqueInJoints.get(i); FrameVector jointAxis = pair.getLeft(); DoubleYoVariable torqueAboutJointAxis = pair.getRight(); tempWrench.changeFrame(jointAxis.getReferenceFrame()); tempFrameVector.setToZero(tempWrench.getExpressedInFrame()); tempWrench.getAngularPart(tempFrameVector); torqueAboutJointAxis.set(-tempFrameVector.dot(jointAxis)); } }
private void setAccelerationEndpointPositions() { for (int i : new int[] {0, 1}) { FrameVector waypointToEndpoint = getWaypointToEndpoint(i); FrameVector oppositeWaypointToEndpoint = getOppositeWaypointToEndpoint(i); double scaleFactor = waypointToEndpoint.dot(oppositeWaypointToEndpoint) / oppositeWaypointToEndpoint.length() * linearSplineLengthFactor.getDoubleValue(); oppositeWaypointToEndpoint.normalize(); oppositeWaypointToEndpoint.scale(scaleFactor); allPositions[accelerationEndpointIndices[i]].set(allPositions[waypointIndices[i]].getFramePointCopy()); allPositions[accelerationEndpointIndices[i]].add(oppositeWaypointToEndpoint); } }
double rotationOnEdge = edgeVector.dot(desiredRotationVector);
public void update() { centerOfMassCalculator.getDesiredFrame().update(); centerOfMassCalculator.compute(); centerOfMassCalculator.getCenterOfMass(centerOfMassPosition); belowJointCoMInZUpFrame.set(centerOfMassPosition); jointAxis.setIncludingFrame(parentJoint.getJointAxis()); jointAxis.changeFrame(parentJoint.getFrameAfterJoint()); jointAxisInWorld.setIncludingFrame(jointAxis); jointAxisInWorld.changeFrame(ReferenceFrame.getWorldFrame()); yoJointAxis.set(jointAxisInWorld); centerOfMassPosition.changeFrame(jointAxis.getReferenceFrame()); jointToCenterOfMass.setIncludingFrame(centerOfMassPosition); jointToCenterOfMassInWorld.setIncludingFrame(jointToCenterOfMass); jointToCenterOfMassInWorld.changeFrame(ReferenceFrame.getWorldFrame()); yoJointToCenterOfMass.set(jointToCenterOfMassInWorld); totalMass.set(centerOfMassCalculator.getTotalMass()); forceVector.setIncludingFrame(ReferenceFrame.getWorldFrame(), 0.0, 0.0, -9.81 * totalMass.getDoubleValue()); forceVector.changeFrame(jointAxis.getReferenceFrame()); FrameVector forceVectorInWorld = new FrameVector(forceVector); forceVectorInWorld.changeFrame(ReferenceFrame.getWorldFrame()); yoForceVector.set(forceVectorInWorld); rCrossFVector.setToZero(jointAxis.getReferenceFrame()); rCrossFVector.cross(forceVector, jointToCenterOfMass); estimatedTorque.set(rCrossFVector.dot(jointAxis)); if (isSpineJoint) estimatedTorque.mul(-1.0); }
if (tempVector.dot(expectedArmZeroConfiguration) > 1.0 - 1e-5)