public void setOrientation(FrameOrientation frameOrientation) { boolean notifyListeners = true; orientation.set(frameOrientation, notifyListeners); }
public void setOrientation(Quat4d quaternion) { orientation.set(quaternion); }
public void set(Quat4d quaternion) { set(quaternion, true); }
public void set(FrameOrientation orientation) { set(orientation, true); }
public void setOrientation(FrameOrientation orientation) { this.yoFrameOrientation.set(orientation); }
public void set(FramePoint framePoint, FrameOrientation frameOrientation) { boolean notifyListeners = true; position.set(framePoint, notifyListeners); orientation.set(frameOrientation, notifyListeners); }
public void set(Quat4d quaternion, boolean notifyListeners) { tempFrameOrientation.setIncludingFrame(getReferenceFrame(), quaternion); set(tempFrameOrientation, notifyListeners); }
public void set(Matrix3d rotation) { tempFrameOrientation.setIncludingFrame(getReferenceFrame(), rotation); set(tempFrameOrientation); }
public void set(RigidBodyTransform transform3D) { tempFrameOrientation.setIncludingFrame(getReferenceFrame(), transform3D); set(tempFrameOrientation); }
public void set(YoFrameQuaternion orientation) { orientation.checkReferenceFrameMatch(getReferenceFrame()); orientation.getFrameOrientationIncludingFrame(tempFrameOrientation); set(tempFrameOrientation); }
private void updateOrientation() { measureOrientationInFusedFrame(firstFrameOrientation, firstIMU); measureOrientationInFusedFrame(secondFrameOrientation, secondIMU); // estimateYawDriftAndCorrectOrientation(firstFrameOrientation, secondFrameOrientation, fusedFrameOrientation); fusedFrameOrientation.interpolate(firstFrameOrientation, secondFrameOrientation, 0.0); orientation.set(fusedFrameOrientation); quaternion.set(fusedFrameOrientation); }
private void updateViz() { yoRootJointFrameQuaternion.checkReferenceFrameMatch(worldFrame); yoRootJointFrameQuaternion.set(rotationFromRootJointFrameToWorld); yoRootJointAngularVelocityFromFD.update(); yoRootJointFrameOrientation.checkReferenceFrameMatch(worldFrame); yoRootJointFrameOrientation.set(rotationFromRootJointFrameToWorld); }
public void interpolate(YoFrameOrientation orientationOne, YoFrameOrientation orientationTwo, double alpha) { orientationOne.putYoValuesIntoFrameOrientation(); orientationTwo.putYoValuesIntoFrameOrientation(); tempFrameOrientation.setToZero(getReferenceFrame()); tempFrameOrientation.interpolate(orientationOne.tempFrameOrientation, orientationTwo.tempFrameOrientation, alpha); this.set(tempFrameOrientation); }
public void set(FramePose framePose, boolean notifyListeners) { framePose.checkReferenceFrameMatch(getReferenceFrame()); framePose.getPositionIncludingFrame(tempFramePoint); framePose.getOrientationIncludingFrame(tempFrameOrientation); position.set(tempFramePoint, notifyListeners); orientation.set(tempFrameOrientation, notifyListeners); }
public void setPose(FramePose framePose) { yoFramePoint.checkReferenceFrameMatch(framePose.getReferenceFrame()); framePose.getPosition(tempPoint); yoFramePoint.setPoint(tempPoint); framePose.getOrientation(tempQuaternion); yoFrameOrientation.set(tempQuaternion); }
public void setAndMatchFrame(FramePose framePose, boolean notifyListeners) { framePose.getPositionIncludingFrame(tempFramePoint); framePose.getOrientationIncludingFrame(tempFrameOrientation); tempFramePoint.changeFrame(getReferenceFrame()); tempFrameOrientation.changeFrame(getReferenceFrame()); position.set(tempFramePoint, notifyListeners); orientation.set(tempFrameOrientation, notifyListeners); }
private void visualizeTrajectory() { for (int i = 0; i < numberOfBalls; i++) { double t = (double) i / ((double) numberOfBalls - 1) * trajectoryTime.getDoubleValue(); compute(t); currentPosition.getFrameTupleIncludingFrame(ballPosition); ballPosition.changeFrame(ReferenceFrame.getWorldFrame()); currentOrientationForViz.set(currentOrientation); bagOfBalls.setBallLoop(ballPosition); } reset(); }
public void update() { FramePoint tempCoMPosition = new FramePoint(worldFrame); for (RigidBodyVisualizationData comData : centerOfMassData) { comData.rigidBody.getCoMOffset(tempCoMPosition); tempCoMPosition.changeFrame(worldFrame); tempCoMPosition.add(inertiaEllipsoidGhostOffset.getFrameVectorCopy()); FrameOrientation orientation = new FrameOrientation(comData.rigidBody.getBodyFixedFrame()); orientation.changeFrame(worldFrame); comData.position.set(tempCoMPosition); comData.orientation.set(orientation); } }
@Override public void onBehaviorEntered() { hasTargetBeenProvided.set(false); haveFootstepsBeenGenerated.set(false); hasInputBeenSet.set(false); footstepListBehavior.initialize(); robotPose.setToZero(fullRobotModel.getRootJoint().getFrameAfterJoint()); robotPose.changeFrame(worldFrame); robotYoPose.set(robotPose); robotPose.getPosition(robotLocation); robotPose.getOrientation(robotOrientation); this.targetLocation.set(robotLocation); this.targetOrientation.set(robotOrientation); }
@Override public void doTransitionIntoAction() { super.doTransitionIntoAction(); // Remember the previous contact normal, in case the foot leaves the ground and rotates holdPositionNormalContactVector.setIncludingFrame(fullyConstrainedNormalContactVector); holdPositionNormalContactVector.changeFrame(worldFrame); momentumBasedController.setPlaneContactStateNormalContactVector(contactableFoot, holdPositionNormalContactVector); desiredSolePosition.setToZero(soleFrame); desiredSolePosition.changeFrame(worldFrame); desiredOrientation.setToZero(soleFrame); desiredOrientation.changeFrame(worldFrame); if (doHoldFootFlatOrientation.getBooleanValue()) { desiredOrientation.setYawPitchRoll(desiredOrientation.getYaw(), 0.0, 0.0); } desiredHoldOrientation.set(desiredOrientation); desiredLinearVelocity.setToZero(worldFrame); desiredAngularVelocity.setToZero(worldFrame); desiredLinearAcceleration.setToZero(worldFrame); desiredAngularAcceleration.setToZero(worldFrame); }