/** * Prepends a rotation about the z-axis to this pose 3D: Rotates the position part and prepends the * rotation to the orientation part. * * @param yaw the angle to rotate about the z-axis. */ default void prependYawRotation(double yaw) { RotationMatrixTools.applyYawRotation(yaw, getPosition(), getPosition()); getOrientation().prependYawRotation(yaw); }
/** * Rotates, then adds the translation (x, y, z) to this pose 3D. * <p> * Use this method if the translation (x, y, z) is expressed in the local coordinates described by * this pose 3D. Otherwise, use {@link #prependTranslation(double, double, double)}. * </p> * * @param x the translation distance along the x-axis. * @param y the translation distance along the y-axis. * @param z the translation distance along the z-axis. */ default void appendTranslation(double x, double y, double z) { double thisX = getX(); double thisY = getY(); double thisZ = getZ(); setPosition(x, y, z); getOrientation().transform(getPosition()); getPosition().add(thisX, thisY, thisZ); }
/** * Sets this pose 3D to the {@code other} pose 3D. * * @param other the other pose 3D. Not modified. */ @Override public void set(Pose3D other) { Pose3DBasics.super.set(other); }
/** * Sets both position and orientation. * * @param position the tuple with the new position coordinates. Not modified. * @param orientation the new orientation. Not modified. */ default void set(Tuple3DReadOnly position, Orientation3DReadOnly orientation) { setOrientation(orientation); setPosition(position); }
/** * Sets all the components of this pose 3D. * <p> * WARNING: the Euler angles or yaw-pitch-roll representation is sensitive to gimbal lock and is * sometimes undefined. * </p> * * @param x the x-coordinate of the position. * @param y the y-coordinate of the position. * @param z the z-coordinate of the position. * @param yaw the angle to rotate about the z-axis. * @param pitch the angle to rotate about the y-axis. * @param roll the angle to rotate about the x-axis. */ default void set(double x, double y, double z, double yaw, double pitch, double roll) { setPosition(x, y, z); setOrientationYawPitchRoll(yaw, pitch, roll); }
/** * Sets the orientation part of this pose 3D with the given orientation. * * @param orientation the orientation used to set this pose's orientation. Not modified. */ default void setOrientation(Orientation3DReadOnly orientation) { getOrientation().set(orientation); }
public void updateFullRobotModel(KinematicsToolboxOutputStatus solution) { if (jointsHashCode != solution.getJointNameHash()) throw new RuntimeException("Hashes are different."); for (int i = 0; i < oneDoFJoints.length; i++) { float q = solution.getDesiredJointAngles().get(i); OneDoFJointBasics joint = oneDoFJoints[i]; joint.setQ(q); } Vector3D translation = solution.getDesiredRootTranslation(); rootJoint.getJointPose().setPosition(translation.getX(), translation.getY(), translation.getZ()); Quaternion orientation = solution.getDesiredRootOrientation(); rootJoint.getJointPose().getOrientation().setQuaternion(orientation.getX(), orientation.getY(), orientation.getZ(), orientation.getS()); fullRobotModelToUseForConversion.updateFrames(); }
/** * Sets the x-coordinate of the position. * * @param x the x-coordinate of the position. */ default void setX(double x) { getPosition().setX(x); }
getRandomVector(random)); sixDoFJoint.setJointPosition(getRandomVector(random)); sixDoFJoint.getJointPose().setOrientationYawPitchRoll(random.nextDouble(), random.nextDouble(), random.nextDouble()); sixDoFJoint.setJointAcceleration(jointAcceleration); elevator.updateFramesRecursively(); rotationMatrix.set(sixDoFJoint.getJointPose().getOrientation());
/** {@inheritDoc} */ @Override default void setJointPosition(Tuple3DReadOnly jointTranslation) { getJointPose().setPosition(jointTranslation); }
private void setRandomPosition(FloatingJoint floatingJoint, SixDoFJoint sixDoFJoint) { Point3D rootPosition = new Point3D(random.nextDouble(), random.nextDouble(), random.nextDouble()); double yaw = random.nextDouble(); double pitch = random.nextDouble(); double roll = random.nextDouble(); floatingJoint.setPosition(rootPosition); floatingJoint.setYawPitchRoll(yaw, pitch, roll); sixDoFJoint.setJointPosition(rootPosition); sixDoFJoint.getJointPose().setOrientationYawPitchRoll(yaw, pitch, roll); }
/** * Sets the orientation from the given frame orientation 2D. * * @param orientation the orientation with the new angle value for this. Not modified. * @throws ReferenceFrameMismatchException if {@code this} and {@code orientation} are not expressed * in the same reference frame. */ default void setOrientation(FrameOrientation2DReadOnly orientation) { checkReferenceFrameMatch(orientation); Pose3DBasics.super.setOrientation(orientation); }
/** * Sets the orientation part of this pose 3D with the given yaw, pitch, and roll angles. * <p> * WARNING: the Euler angles or yaw-pitch-roll representation is sensitive to gimbal lock and is * sometimes undefined. * </p> * * @param yawPitchRoll array containing the yaw-pitch-roll angles. Not modified. * @deprecated Use {@link #setOrientation(Orientation3DReadOnly)} with {@link YawPitchRoll} instead. */ default void setOrientationYawPitchRoll(double[] yawPitchRoll) { getOrientation().setYawPitchRoll(yawPitchRoll); }
rootJoint.getJointPose().setPosition(translation.getX(), translation.getY(), translation.getZ()); Quaternion orientation = robotConfigurationData.getRootOrientation(); rootJoint.getJointPose().getOrientation().setQuaternion(orientation.getX(), orientation.getY(), orientation.getZ(), orientation.getS()); rootJoint.getPredecessor().updateFramesRecursively();
/** * Sets the y-coordinate of the position. * * @param y the y-coordinate of the position. */ default void setY(double y) { getPosition().setY(y); }
/** * Sets this pose 3D to the {@code other} pose 3D. * * @param other the other pose 3D. Not modified. */ default void set(Pose3DReadOnly other) { setPosition(other.getPosition()); setOrientation(other.getOrientation()); }
/** * Sets the position from the given frame tuple 3D. * * @param position the tuple with the new position coordinates. Not modified. * @throws ReferenceFrameMismatchException if {@code this} and {@code position} are not expressed in * the same reference frame. */ default void setPosition(FrameTuple3DReadOnly position) { checkReferenceFrameMatch(position); Pose3DBasics.super.setPosition(position); }
sixDoFJoint.getJointPose().setOrientationYawPitchRoll(random.nextDouble(), random.nextDouble(), random.nextDouble());
/** * Sets the orientation from the given frame orientation. * * @param orientation the orientation to set the orientation part of this frame pose. Not modified. * @throws ReferenceFrameMismatchException if {@code this} and {@code orientation} are not expressed * in the same reference frame. */ default void setOrientation(FrameOrientation3DReadOnly orientation) { checkReferenceFrameMatch(orientation); Pose3DBasics.super.setOrientation(orientation); }
/** * Prepends a rotation about the y-axis to this pose 3D: Rotates the position part and prepends the * rotation to the orientation part. * * @param pitch the angle to rotate about the y-axis. */ default void prependPitchRotation(double pitch) { RotationMatrixTools.applyPitchRotation(pitch, getPosition(), getPosition()); getOrientation().prependPitchRotation(pitch); }