public FrameVector getSensorTorqueRaw(ReferenceFrame desiredFrame) { FrameVector torque = yoSensorTorque.getFrameTuple(); torque.changeFrame(desiredFrame); return torque; }
public FrameVector getSensorForceMassCompensated(ReferenceFrame desiredFrame) { FrameVector force = yoSensorForceMassCompensated.getFrameTuple(); force.changeFrame(desiredFrame); return force; }
public FrameVector getSensorTorqueMassCompensated(ReferenceFrame desiredFrame) { FrameVector torque = yoSensorTorqueMassCompensated.getFrameTuple(); torque.changeFrame(desiredFrame); return torque; }
public void convertToTwist(FrameVector linearVelocityOfOrigin, FrameVector angularVelocity, RigidBody base, Twist twistToPack) { angularVelocity.changeFrame(endEffectorFrame); linearVelocityOfOrigin.changeFrame(endEffectorFrame); twistToPack.set(endEffectorFrame, base.getBodyFixedFrame(), endEffectorFrame, linearVelocityOfOrigin.getVector(), angularVelocity.getVector()); }
public void convertToSpatialAcceleration(FrameVector linearAccelerationOfOrigin, FrameVector angularAcceleration, RigidBody base, SpatialAccelerationVector toPack) { angularAcceleration.changeFrame(endEffectorFrame); linearAccelerationOfOrigin.changeFrame(endEffectorFrame); twistCalculator.getRelativeTwist(twistOfEndEffectorWithRespectToElevator, base, endEffector); twistOfEndEffectorWithRespectToElevator.changeBodyFrameNoRelativeTwist(endEffectorFrame); ReferenceFrame baseFrame = base.getBodyFixedFrame(); toPack.setBasedOnOriginAcceleration(endEffectorFrame, baseFrame, endEffectorFrame, angularAcceleration, linearAccelerationOfOrigin, twistOfEndEffectorWithRespectToElevator); }
public void setAndMatchFrameLinearMomentumRate(FrameVector linearMomentumRate) { this.linearMomentumRate.setIncludingFrame(linearMomentumRate); this.linearMomentumRate.changeFrame(ReferenceFrame.getWorldFrame()); }
public String toStringForASingleReferenceFrame(ReferenceFrame referenceFrame) { getFrameTupleIncludingFrame(frameVector); frameVector.changeFrame(referenceFrame); return frameVector.toString(); }
public void setFinalConditions(YoFramePoint finalPosition, YoFrameVector finalVelocity, ReferenceFrame attachedFrame) { finalPosition.getFrameTupleIncludingFrame(finalPositionInSpecificFrame); finalVelocity.getFrameTupleIncludingFrame(finalVelocityInSpecificFrame); finalPositionInSpecificFrame.changeFrame(attachedFrame); finalVelocityInSpecificFrame.changeFrame(attachedFrame); }
private DenseMatrix64F computeSingleRhoJacobian(FramePoint basisVectorOrigin, FrameVector basisVector) { basisVectorOrigin.changeFrame(centerOfMassFrame); basisVector.changeFrame(centerOfMassFrame); // Compute the unit wrench corresponding to the basis vector unitSpatialForceVector.setIncludingFrame(basisVector, basisVectorOrigin); unitSpatialForceVector.getMatrix(singleRhoJacobian); return singleRhoJacobian; }
public void setInitialTimePositionsAndVelocities() { t0 = startTimeProvider.getValue(); startTime.set(startTimeProvider.getValue()); timeIntoTouchdown.set(0.0); initialPositionSource.getPosition(p0); p0.changeFrame(referenceFrame); velocitySource.get(pd0); pd0.changeFrame(referenceFrame); accelerationSource.get(pdd0); pdd0.changeFrame(referenceFrame); }
private void readAndUpdateRootJointAngularAndLinearVelocity() { ReferenceFrame elevatorFrame = rootJoint.getFrameBeforeJoint(); ReferenceFrame pelvisFrame = rootJoint.getFrameAfterJoint(); FrameVector linearVelocity = robot.getRootJointVelocity(); linearVelocity.changeFrame(pelvisFrame); FrameVector angularVelocity = robot.getRootJointAngularVelocityInRootJointFrame(pelvisFrame); angularVelocity.changeFrame(pelvisFrame); Twist bodyTwist = new Twist(pelvisFrame, elevatorFrame, pelvisFrame, linearVelocity.getVector(), angularVelocity.getVector()); rootJoint.setJointTwist(bodyTwist); }
public void propagateState(double dt) { FrameVector centerOfMassVelocity = centerOfMassVelocityPort.getData(); centerOfMassVelocity.changeFrame(worldFrame); centerOfMassPositionDelta.set(centerOfMassVelocity); centerOfMassPositionDelta.scale(dt); updateCenterOfMassPosition(centerOfMassPositionDelta); }
protected FrameVector computeGroundReactionForce(FramePoint2d cmp2d, double fZ) { centerOfMass.setToZero(centerOfMassFrame); WrenchDistributorTools.computePseudoCMP3d(cmp3d, centerOfMass, cmp2d, fZ, totalMass, omega0); centerOfMass.setToZero(centerOfMassFrame); WrenchDistributorTools.computeForce(groundReactionForce, centerOfMass, cmp3d, fZ); groundReactionForce.changeFrame(centerOfMassFrame); return groundReactionForce; }
public static Vector3d getTransformedVector(Vector3d vector3d, RigidBodyTransform transform3D) { ReferenceFrame ending = ReferenceFrame.constructARootFrame("ending", false, true, true); ReferenceFrame starting = ReferenceFrame.constructFrameWithUnchangingTransformToParent("starting", ending, transform3D, false, true, true); FrameVector framePoint = new FrameVector(starting, vector3d); framePoint.changeFrame(ending); return framePoint.getVector(); }
private void updateMeasurement() { wrenchCalculatorInterface.calculate(); MatrixTools.extractFrameTupleFromEJMLVector(measuredForce, wrenchCalculatorInterface.getWrench(), measurementFrame, 3); measuredForceWorld.setIncludingFrame(measuredForce); measuredForceWorld.changeFrame(worldFrame); yoMeasuredForceWorld.setAndMatchFrame(measuredForce); }
private void computeNormalContactVectorRotation(Matrix3d normalContactVectorRotationMatrixToPack) { yoPlaneContactState.getContactNormalFrameVector(contactNormalVector); contactNormalVector.changeFrame(planeFrame); contactNormalVector.normalize(); GeometryTools.getAxisAngleFromZUpToVector(contactNormalVector.getVector(), normalContactVectorRotation); normalContactVectorRotationMatrixToPack.set(normalContactVectorRotation); }
public void run() { centerOfMassAccelerationCalculator.getCoMAcceleration(comAccelerationFrameVector); comAccelerationFrameVector.changeFrame(ReferenceFrame.getWorldFrame()); comAccelerationFrameVector.get(comAcceleration); if (CORRUPT_DESIRED_ACCELERATIONS) signalCorruptor.corrupt(comAcceleration); comAccelerationFrameVector.set(comAcceleration); desiredCoMAndAngularAccelerationDataSource.setDesiredCenterOfMassAcceleration(comAccelerationFrameVector); } }
public void setBasedOnOriginAcceleration(FrameVector angularAcceleration, FrameVector originAcceleration, Twist twistOfBodyWithRespectToBase) { bodyFrame.checkReferenceFrameMatch(expressedInFrame); twistOfBodyWithRespectToBase.getBodyFrame().checkReferenceFrameMatch(bodyFrame); twistOfBodyWithRespectToBase.getBaseFrame().checkReferenceFrameMatch(baseFrame); angularAcceleration.changeFrame(bodyFrame); angularPart.set(angularAcceleration.getVector()); originAcceleration.changeFrame(bodyFrame); twistOfBodyWithRespectToBase.changeFrame(bodyFrame); linearPart.cross(twistOfBodyWithRespectToBase.getAngularPart(), twistOfBodyWithRespectToBase.getLinearPart()); linearPart.sub(originAcceleration.getVector(), linearPart); }
private void cancelHandWeight(RobotSide robotSide, Wrench wrenchToSubstractHandWeightTo, ReferenceFrame measurementFrame) { CenterOfMassReferenceFrame handCoMFrame = handCenterOfMassFrames.get(robotSide); handCoMFrame.update(); tempWristForce.setIncludingFrame(worldFrame, 0.0, 0.0, -handsMass.get(robotSide).getDoubleValue() * gravity); tempWristForce.changeFrame(handCoMFrame); wristWrenchDueToGravity.setToZero(measurementFrame, handCoMFrame); wristWrenchDueToGravity.setLinearPart(tempWristForce); wristWrenchDueToGravity.changeFrame(measurementFrame); wrenchToSubstractHandWeightTo.sub(wristWrenchDueToGravity); }
private void cancelHandWeight(RobotSide robotSide, Wrench wrenchToSubstractHandWeightTo, ReferenceFrame measurementFrame) { CenterOfMassReferenceFrame handCoMFrame = wristsubtreeCenterOfMassFrames.get(robotSide); handCoMFrame.update(); tempForce.setIncludingFrame(worldFrame, 0.0, 0.0, -wristSubtreeMass.get(robotSide).getDoubleValue() * gravity); tempForce.changeFrame(handCoMFrame); wristWrenchDueToGravity.setToZero(measurementFrame, handCoMFrame); wristWrenchDueToGravity.setLinearPart(tempForce); wristWrenchDueToGravity.changeFrame(measurementFrame); wrenchToSubstractHandWeightTo.sub(wristWrenchDueToGravity); }