public YoQuaternionProvider(String namePrefix, ReferenceFrame referenceFrame, YoVariableRegistry registry) { orientation = new YoFrameQuaternion(namePrefix + "Orientation", referenceFrame, registry); }
public YoSE3OffsetFrame(String frameName, ReferenceFrame parentFrame, YoVariableRegistry registry) { super(frameName, parentFrame); translationToParent = new YoFrameVector(frameName, parentFrame, registry); rotationToParent = new YoFrameQuaternion(frameName, parentFrame, registry); }
public YoSE3ConfigurationProvider(String name, ReferenceFrame frame, YoVariableRegistry registry) { position = new YoFramePoint(name, frame, registry); orientation = new YoFrameQuaternion(name, frame, registry); }
public YoFramePoseUsingQuaternions(String prefix, String suffix, ReferenceFrame frame, YoVariableRegistry registry) { position = new YoFramePoint(prefix, suffix, frame, registry); orientation = new YoFrameQuaternion(prefix, suffix, frame, registry); }
public SimulatedOrientationSensorFromRobot(String name, IMUMount imuMount, YoVariableRegistry registry) { this.imuMount = imuMount; this.yoFrameQuaternionPerfect = new YoFrameQuaternion(name + "Perfect", ReferenceFrame.getWorldFrame(), registry); this.yoFrameQuaternionNoisy = new YoFrameQuaternion(name + "Noisy", ReferenceFrame.getWorldFrame(), registry); rotationAngleNoise = new DoubleYoVariable(name + "Noise", registry); }
public SimulatedOrientationSensor(String name, ReferenceFrame frameUsedForPerfectOrientation, YoVariableRegistry registry) { this.frameUsedForPerfectOrientation = frameUsedForPerfectOrientation; this.yoFrameQuaternionPerfect = new YoFrameQuaternion(name + "Perfect", ReferenceFrame.getWorldFrame(), registry); this.yoFrameQuaternionNoisy = new YoFrameQuaternion(name + "Noisy", ReferenceFrame.getWorldFrame(), registry); rotationAngleNoise = new DoubleYoVariable(name + "Noise", registry); }
private FiniteDifferenceAngularVelocityYoFrameVector(String namePrefix, YoFrameQuaternion orientationToDifferentiate, ReferenceFrame referenceFrame, double dt, YoVariableRegistry registry) { super(namePrefix, referenceFrame, registry); this.dt = dt; orientation = orientationToDifferentiate; orientationPreviousValue = new YoFrameQuaternion(namePrefix + "_previous", referenceFrame, registry); hasBeenCalled = new BooleanYoVariable(namePrefix + "HasBeenCalled", registry); hasBeenCalled.set(false); }
public YoFrameQuaternionControlFlowOutputPort(ControlFlowElement controlFlowElement, String namePrefix, ReferenceFrame frame, YoVariableRegistry registry) { super(namePrefix, controlFlowElement); yoFrameQuaternion = new YoFrameQuaternion(namePrefix, frame, registry); super.setData(new FrameOrientation(frame)); }
public YoFrameQuaternion getOrCreateOrientation(RigidBody endEffector, Type type) { EnumMap<Type, YoFrameQuaternion> typeDependentOrientations = endEffectorOrientations.get(endEffector); if (typeDependentOrientations == null) { typeDependentOrientations = new EnumMap<>(Type.class); endEffectorOrientations.put(endEffector, typeDependentOrientations); } YoFrameQuaternion yoFrameQuaternion = typeDependentOrientations.get(type); if (yoFrameQuaternion == null) { String namePrefix = endEffector.getName(); namePrefix += type.getName(); namePrefix += Space.ORIENTATION.getName(); yoFrameQuaternion = new YoFrameQuaternion(namePrefix, worldFrame, registry); typeDependentOrientations.put(type, yoFrameQuaternion); } return yoFrameQuaternion; }
public ConstantOrientationTrajectoryGenerator(String namePrefix, ReferenceFrame referenceFrame, OrientationProvider orientationProvider, double finalTime, YoVariableRegistry parentRegistry) { MathTools.checkIfInRange(finalTime, 0.0, Double.POSITIVE_INFINITY); this.registry = new YoVariableRegistry(namePrefix + getClass().getSimpleName()); this.orientationProvider = orientationProvider; this.orientation = new YoFrameQuaternion("orientation", referenceFrame, registry); this.finalTime = new DoubleYoVariable("finalTime", registry); this.time = new DoubleYoVariable("time", registry); this.finalTime.set(finalTime); parentRegistry.addChild(registry); }
public OrientationInterpolationTrajectoryGenerator(String namePrefix, ReferenceFrame referenceFrame, DoubleProvider trajectoryTimeProvider, OrientationProvider initialOrientationProvider, OrientationProvider finalOrientationProvider, YoVariableRegistry parentRegistry) { this.registry = new YoVariableRegistry(namePrefix + getClass().getSimpleName()); this.trajectoryTime = new DoubleYoVariable(namePrefix + "TrajectoryTime", registry); this.currentTime = new DoubleYoVariable(namePrefix + "Time", registry); this.parameterPolynomial = new YoPolynomial(namePrefix + "ParameterPolynomial", 6, registry); this.initialOrientation = new YoFrameQuaternion(namePrefix + "InitialOrientation", referenceFrame, registry); this.finalOrientation = new YoFrameQuaternion(namePrefix + "FinalOrientation", referenceFrame, registry); this.continuouslyUpdateFinalOrientation = new BooleanYoVariable(namePrefix + "ContinuouslyUpdate", registry); this.desiredOrientation = new YoFrameQuaternion(namePrefix + "desiredOrientation", referenceFrame, registry); this.desiredAngularVelocity = new YoFrameVector(namePrefix + "desiredAngularVelocity", referenceFrame, registry); this.desiredAngularAcceleration = new YoFrameVector(namePrefix + "desiredAngularAcceleration", referenceFrame, registry); this.trajectoryTimeProvider = trajectoryTimeProvider; this.initialOrientationProvider = initialOrientationProvider; this.finalOrientationProvider = finalOrientationProvider; tempInitialOrientation = new FrameOrientation(referenceFrame); tempFinalOrientation = new FrameOrientation(referenceFrame); parentRegistry.addChild(registry); }
public AbstractDesiredFootstepCalculator(YoVariableRegistry parentRegistry) { for (RobotSide robotSide : RobotSide.values) { String namePrefix = robotSide.getCamelCaseNameForMiddleOfExpression() + "Footstep"; YoFramePoint footstepPosition = new YoFramePoint(namePrefix + "Position", worldFrame, registry); footstepPositions.put(robotSide, footstepPosition); YoFrameQuaternion footstepOrientation = new YoFrameQuaternion(namePrefix + "Orientation", "", worldFrame, registry); footstepOrientations.put(robotSide, footstepOrientation); } parentRegistry.addChild(registry); }
initialOrientation = new YoFrameQuaternion(initialOrientationName, referenceFrame, registry); finalOrientation = new YoFrameQuaternion(finalOrientationName, referenceFrame, registry); currentOrientation = new YoFrameQuaternion(currentOrientationName, referenceFrame, registry); currentAngularVelocity = new YoFrameVector(currentAngularVelocityName, referenceFrame, registry); currentAngularAcceleration = new YoFrameVector(currentAngularAccelerationName, referenceFrame, registry);
public TaskspaceHeadControlState(RigidBody chest, RigidBody head, double[] homeYawPitchRoll, YoOrientationPIDGainsInterface gains, YoVariableRegistry parentRegistry) { super(HeadControlMode.TASKSPACE); this.gains = gains; feedbackControlCommand.set(chest, head); headFrame = head.getBodyFixedFrame(); chestFrame = chest.getBodyFixedFrame(); trajectoryGenerator = new MultipleWaypointsOrientationTrajectoryGenerator("head", true, worldFrame, registry); trajectoryGenerator.registerNewTrajectoryFrame(chestFrame); homeOrientation = new YoFrameQuaternion("headHomeOrientation", chestFrame, registry); homeOrientation.set(homeYawPitchRoll); parentRegistry.addChild(registry); }
public YoFrameQuaternion findYoFrameQuaternion(String quaternionPrefix, String quaternionSuffix, ReferenceFrame quaternionFrame) { DoubleYoVariable qx = (DoubleYoVariable) scs.getVariable(createQxName(quaternionPrefix, quaternionSuffix)); DoubleYoVariable qy = (DoubleYoVariable) scs.getVariable(createQyName(quaternionPrefix, quaternionSuffix)); DoubleYoVariable qz = (DoubleYoVariable) scs.getVariable(createQzName(quaternionPrefix, quaternionSuffix)); DoubleYoVariable qs = (DoubleYoVariable) scs.getVariable(createQsName(quaternionPrefix, quaternionSuffix)); if (qx == null || qy == null || qz == null || qs == null) return null; else return new YoFrameQuaternion(qx, qy, qz, qs, quaternionFrame); } }
public YoFrameQuaternion findYoFrameQuaternion(String quaternionPrefix, String quaternionSuffix, ReferenceFrame quaternionFrame) { DoubleYoVariable qx = (DoubleYoVariable) scs.getVariable(createQxName(quaternionPrefix, quaternionSuffix)); DoubleYoVariable qy = (DoubleYoVariable) scs.getVariable(createQyName(quaternionPrefix, quaternionSuffix)); DoubleYoVariable qz = (DoubleYoVariable) scs.getVariable(createQzName(quaternionPrefix, quaternionSuffix)); DoubleYoVariable qs = (DoubleYoVariable) scs.getVariable(createQsName(quaternionPrefix, quaternionSuffix)); if (qx == null || qy == null || qz == null || qs == null) return null; else return new YoFrameQuaternion(qx, qy, qz, qs, quaternionFrame); } }
public WholeBodyInverseKinematicsBehavior(String namePrefix, FullHumanoidRobotModelFactory fullRobotModelFactory, DoubleYoVariable yoTime, CommunicationBridgeInterface outgoingCommunicationBridge, FullHumanoidRobotModel fullRobotModel) { super(namePrefix, outgoingCommunicationBridge); this.yoTime = yoTime; this.fullRobotModel = fullRobotModel; solutionQualityThreshold = new DoubleYoVariable(behaviorName + "SolutionQualityThreshold", registry); solutionQualityThreshold.set(0.005); isPaused = new BooleanYoVariable(behaviorName + "IsPaused", registry); isStopped = new BooleanYoVariable(behaviorName + "IsStopped", registry); isDone = new BooleanYoVariable(behaviorName + "IsDone", registry); hasSolverFailed = new BooleanYoVariable(behaviorName + "HasSolverFailed", registry); hasSentMessageToController = new BooleanYoVariable(behaviorName + "HasSentMessageToController", registry); currentSolutionQuality = new DoubleYoVariable(behaviorName + "CurrentSolutionQuality", registry); trajectoryTime = new DoubleYoVariable(behaviorName + "TrajectoryTime", registry); timeSolutionSentToController = new DoubleYoVariable(behaviorName + "TimeSolutionSentToController", registry); for (RobotSide robotSide : RobotSide.values) { String side = robotSide.getCamelCaseNameForMiddleOfExpression(); YoFramePoint desiredHandPosition = new YoFramePoint(behaviorName + "Desired" + side + "Hand", worldFrame, registry); yoDesiredHandPositions.put(robotSide, desiredHandPosition); YoFrameQuaternion desiredHandOrientation = new YoFrameQuaternion(behaviorName + "Desired" + side + "Hand", worldFrame, registry); yoDesiredHandOrientations.put(robotSide, desiredHandOrientation); } yoDesiredChestOrientation = new YoFrameQuaternion(behaviorName + "DesiredChest", worldFrame, registry); yoDesiredPelvisOrientation = new YoFrameQuaternion(behaviorName + "DesiredPelvis", worldFrame, registry); outputConverter = new KinematicsToolboxOutputConverter(fullRobotModelFactory); attachNetworkListeningQueue(kinematicsToolboxOutputQueue, KinematicsToolboxOutputStatus.class); clear(); }
public YoRootJointDesiredConfigurationData(FloatingInverseDynamicsJoint rootJoint, YoVariableRegistry parentRegistry) { YoVariableRegistry registry = new YoVariableRegistry("RootJointDesiredConfigurationData"); parentRegistry.addChild(registry); ReferenceFrame frameAfterJoint = rootJoint.getFrameAfterJoint(); String namePrefix = "rootJointLowLevel"; orientation = new YoFrameQuaternion(namePrefix + "DesiredOrientation", worldFrame, registry); position = new YoFramePoint(namePrefix + "DesiredPosition", worldFrame, registry); linearVelocity = new YoFrameVector(namePrefix + "DesiredLinearVelocity", frameAfterJoint, registry); angularVelocity = new YoFrameVector(namePrefix + "DesiredAngularVelocity", frameAfterJoint, registry); linearAcceleration = new YoFrameVector(namePrefix + "DesiredLinearAcceleration", frameAfterJoint, registry); angularAcceleration = new YoFrameVector(namePrefix + "DesiredAngularAcceleration", frameAfterJoint, registry); clear(); }
public IMUBasedPelvisRotationalStateUpdater(FullInverseDynamicsStructure inverseDynamicsStructure, List<? extends IMUSensorReadOnly> imuProcessedOutputs, IMUBiasProvider imuBiasProvider, IMUYawDriftEstimator imuYawDriftEstimator, double dt, YoVariableRegistry parentRegistry) { this.imuBiasProvider = imuBiasProvider; this.imuYawDriftEstimator = imuYawDriftEstimator; checkNumberOfSensors(imuProcessedOutputs); imuProcessedOutput = imuProcessedOutputs.get(0); rootJoint = inverseDynamicsStructure.getRootJoint(); rootJointFrame = rootJoint.getFrameAfterJoint(); twistCalculator = inverseDynamicsStructure.getTwistCalculator(); measurementFrame = imuProcessedOutput.getMeasurementFrame(); measurementLink = imuProcessedOutput.getMeasurementLink(); yoRootJointFrameOrientation = new YoFrameOrientation("estimatedRootJoint", worldFrame, registry); yoRootJointFrameQuaternion = new YoFrameQuaternion("estimatedRootJoint", worldFrame, registry); rootJointYawOffsetFromFrozenState = new DoubleYoVariable("rootJointYawOffsetFromFrozenState", registry); yoRootJointAngularVelocity = new YoFrameVector("estimatedRootJointAngularVelocity", rootJointFrame, registry); yoRootJointAngularVelocityInWorld = new YoFrameVector("estimatedRootJointAngularVelocityWorld", worldFrame, registry); yoRootJointAngularVelocityMeasFrame = new YoFrameVector("estimatedRootJointAngularVelocityMeasFrame", measurementFrame, registry); yoRootJointAngularVelocityFromFD = new FiniteDifferenceAngularVelocityYoFrameVector("estimatedRootJointAngularVelocityFromFD", yoRootJointFrameQuaternion, dt, registry); parentRegistry.addChild(registry); angularVelocityRootJointFrameRelativeToWorld = new FrameVector(rootJointFrame); }
quaternion = new YoFrameQuaternion(sensorName, fusedMeasurementFrame, registry); orientation = new YoFrameOrientation(sensorName, fusedMeasurementFrame, registry); angularVelocity = new YoFrameVector("qd_w", sensorName, fusedMeasurementFrame, registry);