public void cross(Vector3d vector) { cross(this.tuple, this.tuple, vector); }
public void cross(Vector3d tuple1, Vector3d tuple2) { cross(this.tuple, tuple1, tuple2); }
public void cross(FrameTuple<?, ?> frameTuple1) { checkReferenceFrameMatch(frameTuple1); cross(this.tuple, this.tuple, frameTuple1.tuple); }
public void cross(FrameTuple<?, ?> frameTuple1, FrameTuple<?, ?> frameTuple2) { checkReferenceFrameMatch(frameTuple1); checkReferenceFrameMatch(frameTuple2); cross(this.tuple, frameTuple1.tuple, frameTuple2.tuple); }
public void cross(FrameVector vector1, FrameVector vector2) { getFrameTuple().cross(vector1.getVector(), vector2.getVector()); getYoValuesFromFrameTuple(); }
/** * Packs the linear velocity of a point that is fixed in bodyFrame but is expressed in baseFrame, * with respect to baseFrame, expressed in expressedInFrame */ public void getLinearVelocityOfPointFixedInBodyFrame(FrameVector linearVelocityToPack, FramePoint pointFixedInBodyFrame) { baseFrame.checkReferenceFrameMatch(expressedInFrame); pointFixedInBodyFrame.checkReferenceFrameMatch(baseFrame); pointFixedInBodyFrame.get(freeVector); linearVelocityToPack.setToZero(expressedInFrame); linearVelocityToPack.cross(angularPart, freeVector); linearVelocityToPack.add(linearPart); }
/** * Packs the linear velocity of a point2d that is fixed in bodyFrame but is expressed in baseFrame, * with respect to baseFrame, expressed in expressedInFrame */ public void getLineaVelocityOfPoint2dFixedInBodyFrame(FrameVector linearVelocityToPack, FramePoint2d point2dFixedInBodyFrame) { baseFrame.checkReferenceFrameMatch(expressedInFrame); point2dFixedInBodyFrame.checkReferenceFrameMatch(baseFrame); point2dFixedInBodyFrame.get(freeVector); linearVelocityToPack.setToZero(expressedInFrame); linearVelocityToPack.cross(angularPart, freeVector); linearVelocityToPack.add(linearPart); }
public void cross(YoFrameVector yoFrameVector1, YoFrameVector yoFrameVector2) { getFrameTuple().cross(yoFrameVector1.getFrameTuple().getVector(), yoFrameVector2.getFrameTuple().getVector()); getYoValuesFromFrameTuple(); }
private void getCorrectionVelocityForMeasurementFrameOffset(FrameVector correctionTermToPack) { rootJoint.getJointTwist(tempRootJointTwist); tempRootJointTwist.getAngularPart(tempRootJointAngularVelocity); measurementOffset.setToZero(measurementFrame); measurementOffset.changeFrame(rootJoint.getFrameAfterJoint()); correctionTermToPack.setToZero(tempRootJointAngularVelocity.getReferenceFrame()); correctionTermToPack.cross(tempRootJointAngularVelocity, measurementOffset); } }
private void doYoVectorCrossProduct(YoFrameVector v1, YoFrameVector v2, YoFrameVector vecToPack) { temp.cross(v1.getFrameTuple(), v2.getFrameTuple()); if (temp.length() > 0) { temp.normalize(); } vecToPack.set(world, temp.getX(), temp.getY(), temp.getZ()); } }
private void computeRpd(FrameVector rPdToPack, TwistCalculator twistCalculator, FramePoint rP, FrameVector rd) { // T_{p}^{p,w} twistCalculator.getTwistOfBody(twistOfEstimationLink, estimationLink); twistOfEstimationLink.changeFrame(estimationFrame); // \dot{r}^{p} = R_{w}^{p} \dot{r} - \tilde{\omega}r^{p} - v_{p}^{p,w} rPdToPack.setIncludingFrame(rd); rPdToPack.changeFrame(estimationFrame); twistOfEstimationLink.getAngularPart(tempFrameVector); tempFrameVector.cross(tempFrameVector, rP); rPdToPack.sub(tempFrameVector); twistOfEstimationLink.getLinearPart(tempFrameVector); rPdToPack.sub(tempFrameVector); }
public void getLinearAccelerationFromOriginAcceleration(Twist twistOfBodyWithRespectToBase, FrameVector linearAccelerationToPack) { bodyFrame.checkReferenceFrameMatch(expressedInFrame); twistOfBodyWithRespectToBase.getBodyFrame().checkReferenceFrameMatch(bodyFrame); twistOfBodyWithRespectToBase.getBaseFrame().checkReferenceFrameMatch(baseFrame); twistOfBodyWithRespectToBase.changeFrame(bodyFrame); linearAccelerationToPack.setToZero(bodyFrame); linearAccelerationToPack.cross(twistOfBodyWithRespectToBase.getAngularPart(), twistOfBodyWithRespectToBase.getLinearPart()); linearAccelerationToPack.add(linearPart); }
tempComBody.changeFrame(rootJointFrame); tempCrossPart.setToZero(rootJointFrame); tempCrossPart.cross(rootJointAngularVelocity, tempComBody); tempCrossPart.cross(rootJointAngularVelocity, tempCenterOfMassVelocityWorld); rootJointAccelerationToPack.sub(tempCrossPart); tempCrossPart.cross(tempAngularAcceleration, tempComBody); rootJointAccelerationToPack.sub(tempCrossPart); tempCrossPart.cross(rootJointAngularVelocity, tempComVelocityBody); rootJointAccelerationToPack.sub(tempCrossPart);
private void computeRootJointLinearVelocity(FrameVector centerOfMassVelocityWorld, FrameVector rootJointVelocityToPack, FrameVector rootJointAngularVelocity, FloatingInverseDynamicsJoint rootJoint) { tempCenterOfMassVelocityWorld.setIncludingFrame(centerOfMassVelocityWorld); ReferenceFrame rootJointFrame = rootJoint.getFrameAfterJoint(); // \dot{r}^{root} centerOfMassJacobianBody.compute(); tempComVelocityBody.setToZero(rootJointFrame); centerOfMassJacobianBody.getCenterOfMassVelocity(tempComVelocityBody); tempComVelocityBody.changeFrame(rootJointFrame); // \tilde{\omega} r^{root} tempComBody.setToZero(rootJointFrame); centerOfMassCalculator.getCenterOfMass(tempComBody); tempComBody.changeFrame(rootJointFrame); tempCrossPart.setToZero(rootJointFrame); tempCrossPart.cross(rootJointAngularVelocity, tempComBody); // v_{r/p}= \tilde{\omega} r^{root} + \dot{r}^{root} tempCenterOfMassVelocityOffset.setToZero(rootJointFrame); tempCenterOfMassVelocityOffset.add(tempCrossPart, tempComVelocityBody); // v_{root}^{p,w} = R_{w}^{root} \dot{r} - v_{r/p} tempCenterOfMassVelocityWorld.changeFrame(rootJointFrame); rootJointVelocityToPack.setIncludingFrame(tempCenterOfMassVelocityWorld); rootJointVelocityToPack.sub(tempCenterOfMassVelocityOffset); }
tempFrameVector.cross(omegaEstimationToMeasurement, rP); tempFrameVector.add(vEstimationToMeasurement); tempFrameVector.sub(rPd); tempFrameVector.cross(tempFrameVector, rP); s.add(tempFrameVector); tempFrameVector.cross(tempFrameVector, rPd); s.add(tempFrameVector); tempFrameVector.cross(omega, tempFrameVector); s.add(tempFrameVector); tempFrameVector.cross(tempFrameVector, rP); tempFrameVector.cross(omega, tempFrameVector); s.add(tempFrameVector); s.changeFrame(ReferenceFrame.getWorldFrame());
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); }
tempEstimationLinkAngularVelocity.changeFrame(estimationFrame); tempCrossTerm.setToZero(estimationFrame); tempCrossTerm.cross(tempRootToEstimationAngularVelocity, tempEstimationLinkAngularVelocity);
cross.cross(forceSensorLocation, footForce);