@Override public void getAcceleration(FrameVector accelerationToPack) { accelerationToPack.setToZero(trajectoryFrame); }
@Override public void setDesiredAccelerationToZero() { jointAngularAccelerationDesired.setToZero(jointAngularAccelerationDesired.getReferenceFrame()); }
@Override public void getLinearAccelerationBiasInIMUFrame(IMUSensorReadOnly imu, FrameVector linearAccelerationBiasToPack) { Integer imuIndex = imuToIndexMap.get(imu); if (!enableIMUBiasCompensation.getBooleanValue() || imuIndex == null) linearAccelerationBiasToPack.setToZero(imu.getMeasurementFrame()); else linearAccelerationBiases.get(imuIndex.intValue()).getFrameTupleIncludingFrame(linearAccelerationBiasToPack); }
@Override public void getAngularVelocityBiasInIMUFrame(IMUSensorReadOnly imu, FrameVector angularVelocityBiasToPack) { Integer imuIndex = imuToIndexMap.get(imu); if (!enableIMUBiasCompensation.getBooleanValue() || imuIndex == null) angularVelocityBiasToPack.setToZero(imu.getMeasurementFrame()); else angularVelocityBiases.get(imuIndex.intValue()).getFrameTupleIncludingFrame(angularVelocityBiasToPack); }
/** * Packs a version of the linear velocity, rotated to the base frame. */ public void getBodyOriginLinearPartInBaseFrame(FrameVector linearVelocityAtBodyOriginToPack) { linearVelocityAtBodyOriginToPack.setToZero(baseFrame); getBodyOriginLinearPartInBaseFrame(linearVelocityAtBodyOriginToPack.getVector()); }
public void getAcceleration(FrameVector accelerationToPack) { accelerationToPack.setToZero(referenceFrame); // 2 * c2 c2.getFrameTuple(accelerationToPack); accelerationToPack.scale(2.0); }
/** * Packs the angular velocity of the body frame with respect to the base frame, expressed in the base frame. * The vector is computed by simply rotating the angular velocity part of this twist to base frame. */ public void getAngularVelocityInBaseFrame(FrameVector vectorToPack) { vectorToPack.setToZero(baseFrame); getAngularVelocityInBaseFrame(vectorToPack.getVector()); }
public static void getVelocity(FrameVector velocityToPack, ReferenceFrame frame, double angle, double angleDot, double radius, double radiusDot, double zDot) { double cos = Math.cos(angle); double sin = Math.sin(angle); velocityToPack.setToZero(frame); double xDot = -radius * sin * angleDot + cos * radiusDot; double yDot = radius * cos * angleDot + sin * radiusDot; velocityToPack.setX(xDot); velocityToPack.setY(yDot); velocityToPack.setZ(zDot); }
public void getLinearVelocityIncludingFrame(FrameVector linearVelocityToPack) { linearVelocityToPack.setToZero(getReferenceFrame()); geometryObject.getLinearVelocity(linearVelocityToPack.getVector()); }
public void getLinearVelocityIncludingFrame(FrameVector linearVelocityToPack) { linearVelocityToPack.setToZero(getReferenceFrame()); geometryObject.getLinearVelocity(linearVelocityToPack.getVector()); }
public void getAngularVelocityIncludingFrame(FrameVector angularVelocityToPack) { angularVelocityToPack.setToZero(getReferenceFrame()); geometryObject.getAngularVelocity(angularVelocityToPack.getVector()); }
public void getLinearVelocityIncludingFrame(FrameVector linearVelocityToPack) { linearVelocityToPack.setToZero(getReferenceFrame()); geometryObject.getLinearVelocity(linearVelocityToPack.getVector()); }
public void getAngularVelocityIncludingFrame(FrameVector angularVelocityToPack) { angularVelocityToPack.setToZero(getReferenceFrame()); geometryObject.getAngularVelocity(angularVelocityToPack.getVector()); }
public void getPose(FrameVector translationFromVoxelOrigin, FrameOrientation orientation, int rayIndex, int rotationAroundRayIndex) { MathTools.checkIfInRange(rayIndex, 0, numberOfRays - 1); MathTools.checkIfInRange(rotationAroundRayIndex, 0, numberOfRotationsAroundRay - 1); if (type == SphereVoxelType.graspAroundSphere) translationFromVoxelOrigin.setIncludingFrame(parentFrame, pointsOnSphere[rayIndex]); else translationFromVoxelOrigin.setToZero(parentFrame); orientation.setIncludingFrame(parentFrame, rotations[rayIndex][rotationAroundRayIndex]); }
public void getVelocity(FrameVector velocityToPack, double parameter) { double q = parameter; MathTools.checkIfInRange(q, 0.0, 1.0); velocityToPack.setToZero(referenceFrame); // 2 * c2 * q c2.getFrameTuple(velocityToPack); velocityToPack.scale(2.0 * q); // c1 c1.getFrameTuple(tempPackVelocity); velocityToPack.add(tempPackVelocity); }
private void interpolateVectorFromInitialToFinalFrame(FrameVector vectorTrajectoryFrameToPack, FrameVector vectorInitialFrame, FrameVector vectorFinalFrame, double percentOfFinal) { vectorA.setIncludingFrame(vectorInitialFrame); vectorB.setIncludingFrame(vectorFinalFrame); vectorA.changeFrame(trajectoryFrame); vectorB.changeFrame(trajectoryFrame); vectorTrajectoryFrameToPack.setToZero(trajectoryFrame); vectorTrajectoryFrameToPack.interpolate(vectorA, vectorB, percentOfFinal); }
public void handleGoHome(double trajectoryTime, FrameOrientation initialOrientation) { initialOrientation.changeFrame(chestFrame); homeOrientation.getFrameOrientationIncludingFrame(desiredOrientation); desiredAngularVelocity.setToZero(chestFrame); trajectoryGenerator.clear(); trajectoryGenerator.switchTrajectoryFrame(chestFrame); trajectoryGenerator.appendWaypoint(0.0, initialOrientation, desiredAngularVelocity); trajectoryGenerator.appendWaypoint(trajectoryTime, desiredOrientation, desiredAngularVelocity); }
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); } }
public void compute(FrameVector outputToPack, FrameOrientation desiredOrientation, FrameVector desiredAngularVelocity, FrameVector feedForwardAngularAcceleration, RigidBody base) { // using twists is a bit overkill; optimize if needed. twistCalculator.getRelativeTwist(endEffectorTwist, base, endEffector); currentAngularVelocity.setToZero(endEffectorTwist.getExpressedInFrame()); endEffectorTwist.getAngularPart(currentAngularVelocity); desiredAngularVelocity.changeFrame(currentAngularVelocity.getReferenceFrame()); feedForwardAngularAcceleration.changeFrame(endEffectorTwist.getExpressedInFrame()); axisAngleOrientationController.compute(outputToPack, desiredOrientation, desiredAngularVelocity, currentAngularVelocity, feedForwardAngularAcceleration); }
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); }