diff --git a/include/maliput/drake/common/autodiff.h b/include/maliput/drake/common/autodiff.h index feb863b..1dbe6b5 100644 --- a/include/maliput/drake/common/autodiff.h +++ b/include/maliput/drake/common/autodiff.h @@ -1,4 +1,7 @@ #pragma once + +#define MALIPUT_USED + /// @file This header provides a single inclusion point for autodiff-related /// header files in the `drake/common` directory. Users should include this /// file. Including other individual headers such as `drake/common/autodiffxd.h` diff --git a/include/maliput/drake/common/default_scalars.h b/include/maliput/drake/common/default_scalars.h index fe77731..f33d4bb 100644 --- a/include/maliput/drake/common/default_scalars.h +++ b/include/maliput/drake/common/default_scalars.h @@ -1,5 +1,6 @@ #pragma once +#define MALIPUT_USED // N.B. `CommonScalarPack` and `NonSymbolicScalarPack` in `systems_pybind.h` // should be kept in sync with this file. diff --git a/include/maliput/drake/common/drake_assert.h b/include/maliput/drake/common/drake_assert.h index 3ece42c..36b653f 100644 --- a/include/maliput/drake/common/drake_assert.h +++ b/include/maliput/drake/common/drake_assert.h @@ -1,5 +1,7 @@ #pragma once +#define MALIPUT_USED + #include /// @file diff --git a/include/maliput/drake/common/drake_bool.h b/include/maliput/drake/common/drake_bool.h index b7fd850..b593b69 100644 --- a/include/maliput/drake/common/drake_bool.h +++ b/include/maliput/drake/common/drake_bool.h @@ -1,5 +1,7 @@ #pragma once +#define MALIPUT_USED + #include #include diff --git a/include/maliput/drake/common/drake_copyable.h b/include/maliput/drake/common/drake_copyable.h index 2fb63e9..fe95940 100644 --- a/include/maliput/drake/common/drake_copyable.h +++ b/include/maliput/drake/common/drake_copyable.h @@ -1,5 +1,7 @@ #pragma once +#define MALIPUT_USED + // ============================================================================ // N.B. The spelling of the macro names between doc/Doxyfile_CXX.in and this // file must be kept in sync! diff --git a/include/maliput/drake/common/drake_deprecated.h b/include/maliput/drake/common/drake_deprecated.h index 5ce6328..b789dcb 100644 --- a/include/maliput/drake/common/drake_deprecated.h +++ b/include/maliput/drake/common/drake_deprecated.h @@ -1,5 +1,7 @@ #pragma once +#define MALIPUT_USED + /** @file Provides a portable macro for use in generating compile-time warnings for use of code that is permitted but discouraged. */ diff --git a/include/maliput/drake/common/drake_marker.h b/include/maliput/drake/common/drake_marker.h deleted file mode 100644 index d950dd9..0000000 --- a/include/maliput/drake/common/drake_marker.h +++ /dev/null @@ -1,16 +0,0 @@ -#pragma once - -/// @file -/// This is an internal (not installed) header. Do not use this outside of -/// `find_resource.cc`. - -namespace maliput::drake { -namespace internal { - -// Provides a concrete object to ensure that this marker library is linked. -// This returns a simple magic constant to ensure the library was loaded -// correctly. -int drake_marker_lib_check(); - -} // namespace internal -} // namespace maliput::drake diff --git a/include/maliput/drake/common/drake_throw.h b/include/maliput/drake/common/drake_throw.h index 1a72fdc..d08f235 100644 --- a/include/maliput/drake/common/drake_throw.h +++ b/include/maliput/drake/common/drake_throw.h @@ -1,5 +1,7 @@ #pragma once +#define MALIPUT_USED + #include #include "maliput/drake/common/drake_assert.h" diff --git a/include/maliput/drake/common/eigen_types.h b/include/maliput/drake/common/eigen_types.h index ecbd644..efe124f 100644 --- a/include/maliput/drake/common/eigen_types.h +++ b/include/maliput/drake/common/eigen_types.h @@ -1,5 +1,7 @@ #pragma once +#define MALIPUT_USED + /// @file /// This file contains abbreviated definitions for certain specializations of /// Eigen::Matrix that are commonly used in Drake. diff --git a/include/maliput/drake/common/extract_double.h b/include/maliput/drake/common/extract_double.h index 35e5e3f..98eb465 100644 --- a/include/maliput/drake/common/extract_double.h +++ b/include/maliput/drake/common/extract_double.h @@ -1,5 +1,8 @@ #pragma once + +#define MALIPUT_USED + #include #include "maliput/drake/common/drake_deprecated.h" diff --git a/include/maliput/drake/common/nice_type_name.h b/include/maliput/drake/common/nice_type_name.h index 3b920a8..b325ace 100644 --- a/include/maliput/drake/common/nice_type_name.h +++ b/include/maliput/drake/common/nice_type_name.h @@ -1,5 +1,7 @@ #pragma once +#define MALIPUT_USED + #include #include #include diff --git a/include/maliput/drake/common/symbolic.h b/include/maliput/drake/common/symbolic.h index 02bc960..af11fe4 100644 --- a/include/maliput/drake/common/symbolic.h +++ b/include/maliput/drake/common/symbolic.h @@ -1,4 +1,7 @@ #pragma once + +#define MALIPUT_USED + /// @file /// Provides public header files of Drake's symbolic library. /// A user of the symbolic library should only include this header file. diff --git a/include/maliput/drake/common/text_logging.h b/include/maliput/drake/common/text_logging.h index 880e0b4..2022cab 100644 --- a/include/maliput/drake/common/text_logging.h +++ b/include/maliput/drake/common/text_logging.h @@ -1,5 +1,8 @@ #pragma once + +#define MALIPUT_USED + /** @file This is the entry point for all text logging within Drake. diff --git a/include/maliput/drake/common/trajectories/bspline_trajectory.h b/include/maliput/drake/common/trajectories/bspline_trajectory.h deleted file mode 100644 index 235da4f..0000000 --- a/include/maliput/drake/common/trajectories/bspline_trajectory.h +++ /dev/null @@ -1,144 +0,0 @@ -#pragma once - -#include -#include - -#include "maliput/drake/common/drake_bool.h" -#include "maliput/drake/common/drake_copyable.h" -#include "maliput/drake/common/drake_throw.h" -#include "maliput/drake/common/eigen_types.h" -#include "maliput/drake/common/name_value.h" -#include "maliput/drake/common/trajectories/trajectory.h" -#include "maliput/drake/math/bspline_basis.h" - -namespace maliput::drake { -namespace trajectories { -/** Represents a B-spline curve using a given `basis` with ordered -`control_points` such that each control point is a matrix in ℝʳᵒʷˢ ˣ ᶜᵒˡˢ. -@see math::BsplineBasis */ -template -class BsplineTrajectory final : public trajectories::Trajectory { - public: - DRAKE_DEFAULT_COPY_AND_MOVE_AND_ASSIGN(BsplineTrajectory); - - BsplineTrajectory() : BsplineTrajectory({}, {}) {} - - /** Constructs a B-spline trajectory with the given `basis` and - `control_points`. - @pre control_points.size() == basis.num_basis_functions() */ - BsplineTrajectory(math::BsplineBasis basis, - std::vector> control_points); - -#ifdef DRAKE_DOXYGEN_CXX - /** Constructs a T-valued B-spline trajectory from a double-valued `basis` and - T-valued `control_points`. - @pre control_points.size() == basis.num_basis_functions() */ - BsplineTrajectory(math::BsplineBasis basis, - std::vector> control_points); -#else - template - BsplineTrajectory(math::BsplineBasis basis, - std::vector> control_points, - typename std::enable_if_t>* = {}) - : BsplineTrajectory(math::BsplineBasis(basis), control_points) {} -#endif - - virtual ~BsplineTrajectory() = default; - - // Required methods for trajectories::Trajectory interface. - std::unique_ptr> Clone() const override; - - /** Evaluates the BsplineTrajectory at the given time t. - @param t The time at which to evaluate the %BsplineTrajectory. - @return The matrix of evaluated values. - @pre If T == symbolic::Expression, `t.is_constant()` must be true. - @warning If t does not lie in the range [start_time(), end_time()], the - trajectory will silently be evaluated at the closest - valid value of time to t. For example, `value(-1)` will return - `value(0)` for a trajectory defined over [0, 1]. */ - MatrixX value(const T& time) const override; - - Eigen::Index rows() const override { return control_points()[0].rows(); } - - Eigen::Index cols() const override { return control_points()[0].cols(); } - - T start_time() const override { return basis_.initial_parameter_value(); } - - T end_time() const override { return basis_.final_parameter_value(); } - - // Other methods - /** Returns the number of control points in this curve. */ - int num_control_points() const { return basis_.num_basis_functions(); } - - /** Returns the control points of this curve. */ - const std::vector>& control_points() const { - return control_points_; - } - - /** Returns this->value(this->start_time()) */ - MatrixX InitialValue() const; - - /** Returns this->value(this->end_time()) */ - MatrixX FinalValue() const; - - /** Returns the basis of this curve. */ - const math::BsplineBasis& basis() const { return basis_; } - - /** Adds new knots at the specified `additional_knots` without changing the - behavior of the trajectory. The basis and control points of the trajectory are - adjusted such that it produces the same value for any valid time before and - after this method is called. The resulting trajectory is guaranteed to have - the same level of continuity as the original, even if knot values are - duplicated. Note that `additional_knots` need not be sorted. - @pre start_time() <= t <= end_time() for all t in `additional_knots` */ - void InsertKnots(const std::vector& additional_knots); - - /** Returns a new BsplineTrajectory that uses the same basis as `this`, and - whose control points are the result of calling `select(point)` on each `point` - in `this->control_points()`.*/ - BsplineTrajectory CopyWithSelector( - const std::function(const MatrixX&)>& select) const; - - /** Returns a new BsplineTrajectory that uses the same basis as `this`, and - whose control points are the result of calling - `point.block(start_row, start_col, block_rows, block_cols)` on each `point` - in `this->control_points()`.*/ - BsplineTrajectory CopyBlock(int start_row, int start_col, - int block_rows, int block_cols) const; - - /** Returns a new BsplineTrajectory that uses the same basis as `this`, and - whose control points are the result of calling `point.head(n)` on each `point` - in `this->control_points()`. - @pre this->cols() == 1 - @pre control_points()[0].head(n) must be a valid operation. */ - BsplineTrajectory CopyHead(int n) const; - - boolean operator==(const BsplineTrajectory& other) const; - - /** Passes this object to an Archive; see @ref serialize_tips for background. - This method is only available when T = double. */ - template -#ifdef DRAKE_DOXYGEN_CXX - void -#else - // Restrict this method to T = double only; we must mix "Archive" into the - // conditional type for SFINAE to work, so we just check it against void. - std::enable_if_t && !std::is_void_v> -#endif - Serialize(Archive* a) { - a->Visit(MakeNameValue("basis", &basis_)); - a->Visit(MakeNameValue("control_points", &control_points_)); - MALIPUT_DRAKE_THROW_UNLESS(CheckInvariants()); - } - - private: - std::unique_ptr> DoMakeDerivative( - int derivative_order) const override; - - bool CheckInvariants() const; - - math::BsplineBasis basis_; - std::vector> control_points_; -}; -} // namespace trajectories -} // namespace maliput::drake diff --git a/include/maliput/drake/common/trajectories/discrete_time_trajectory.h b/include/maliput/drake/common/trajectories/discrete_time_trajectory.h deleted file mode 100644 index f0978da..0000000 --- a/include/maliput/drake/common/trajectories/discrete_time_trajectory.h +++ /dev/null @@ -1,139 +0,0 @@ -#pragma once - -#include -#include -#include - -#include - -#include "maliput/drake/common/drake_assert.h" -#include "maliput/drake/common/drake_copyable.h" -#include "maliput/drake/common/eigen_types.h" -#include "maliput/drake/common/trajectories/piecewise_polynomial.h" -#include "maliput/drake/common/trajectories/trajectory.h" - -namespace maliput::drake { -namespace trajectories { - -/** A DiscreteTimeTrajectory is a Trajectory whose value is only defined at -discrete time points. Calling `value()` at a time that is not equal to one of -those times (up to a tolerance) will throw. This trajectory does *not* have -well-defined time-derivatives. - -In some applications, it may be preferable to use -PiecewisePolynomial::ZeroOrderHold instead of a DiscreteTimeTrajectory (and -we offer a method here to easily convert). Note if the breaks are periodic, -then one can also achieve a similar result in a Diagram by using the -DiscreteTimeTrajectory in a TrajectorySource and connecting a ZeroOrderHold -system to the output port, but remember that this will add discrete state to -your diagram. - -So why not always use the zero-order hold (ZOH) trajectory? This class forces -us to be more precise in our implementations. For instance, consider the case -of a solution to a discrete-time finite-horizon linear quadratic regulator -(LQR) problem. In this case, the solution to the Riccati equation is a -DiscreteTimeTrajectory, K(t). Implementing -@verbatim -x(t) -> MatrixGain(-K(t)) -> u(t) -@verbatim -in a block diagram is perfectly correct, and if the u(t) is only connected to -the original system that it was designed for, then K(t) will only get evaluated -at the defined sample times, and all is well. But if you wire it up to a -continuous-time system, then K(t) may be evaluated at arbitrary times, and may -throw. If one wishes to use the K(t) solution on a continuous-time system, then -we can use -@verbatim -x(t) -> MatrixGain(-K(t)) -> ZOH -> u(t). -@verbatim -This is different, and *more correct* than implementing K(t) as a zero-order -hold trajectory, because in this version, both K(t) and the inputs x(t) will -only be evaluated at the discrete-time input. If `t_s` was the most recent -discrete sample time, then this means u(t) = -K(t_s)*x(t_s) instead of u(t) = --K(t_s)*x(t). Using x(t_s) and having a true zero-order hold on u(t) is the -correct model for the discrete-time LQR result. - -@tparam_default_scalars -*/ -template -class DiscreteTimeTrajectory final : public Trajectory { - public: - // We are final, so this is okay. - DRAKE_DEFAULT_COPY_AND_MOVE_AND_ASSIGN(DiscreteTimeTrajectory) - - /** Default constructor creates the empty trajectory. */ - DiscreteTimeTrajectory() = default; - - /** Constructs a trajectory of vector @p values at the specified @p times. - @pre @p times must differ by more than @p time_comparison_tolerance and be - monotonically increasing. - @pre @p values must have times.size() columns. - @pre @p time_comparison_tolerance must be >= 0. - @throw if T=symbolic:Expression and @p times are not constants. */ - DiscreteTimeTrajectory(const Eigen::Ref>& times, - const Eigen::Ref>& values, - double time_comparison_tolerance = - std::numeric_limits::epsilon()); - - /** Constructs a trajectory of matrix @p values at the specified @p times. - @pre @p times should differ by more than @p time_comparison_tolerance and be - monotonically increasing. - @pre @p values must have times.size() elements, each with the same number of - rows and columns. - @pre @p time_comparison_tolerance must be >= 0. - @throw if T=symbolic:Expression and @p times are not constants. */ - DiscreteTimeTrajectory(const std::vector& times, - const std::vector>& values, - double time_comparison_tolerance = - std::numeric_limits::epsilon()); - - /** Converts the discrete-time trajectory using - PiecewisePolynomial::ZeroOrderHold(). */ - PiecewisePolynomial ToZeroOrderHold() const; - - /** The trajectory is only defined at finite sample times. This method - returns the tolerance used determine which time sample (if any) matches a - query time on calls to value(t). */ - double time_comparison_tolerance() const; - - /** Returns the number of discrete times where the trajectory value is - defined. */ - int num_times() const; - - /** Returns the times where the trajectory value is defined. */ - const std::vector& get_times() const; - - /** Returns a deep copy of the trajectory. */ - std::unique_ptr> Clone() const override; - - /** Returns the value of the trajectory at @p t. - @throws std::exception if t is not within tolerance of one of the sample - times. */ - MatrixX value(const T& t) const override; - - /** Returns the number of rows in the MatrixX returned by value(). - @pre num_times() > 0. */ - Eigen::Index rows() const override; - - /** Returns the number of cols in the MatrixX returned by value(). - @pre num_times() > 0. */ - Eigen::Index cols() const override; - - /** Returns the minimum value of get_times(). - @pre num_times() > 0. */ - T start_time() const override; - - /** Returns the maximum value of get_times(). - @pre num_times() > 0. */ - T end_time() const override; - - private: - std::vector times_; - std::vector> values_; - double time_comparison_tolerance_{}; -}; - -} // namespace trajectories -} // namespace maliput::drake - -DRAKE_DECLARE_CLASS_TEMPLATE_INSTANTIATIONS_ON_DEFAULT_SCALARS( - class maliput::drake::trajectories::DiscreteTimeTrajectory) diff --git a/include/maliput/drake/common/trajectories/exponential_plus_piecewise_polynomial.h b/include/maliput/drake/common/trajectories/exponential_plus_piecewise_polynomial.h deleted file mode 100644 index a8955d3..0000000 --- a/include/maliput/drake/common/trajectories/exponential_plus_piecewise_polynomial.h +++ /dev/null @@ -1,82 +0,0 @@ -#pragma once - -#include -#include - -#include - -#include "maliput/drake/common/drake_assert.h" -#include "maliput/drake/common/drake_copyable.h" -#include "maliput/drake/common/eigen_types.h" -#include "maliput/drake/common/trajectories/piecewise_polynomial.h" - -namespace maliput::drake { -namespace trajectories { - -/** - * y(t) = K * exp(A * (t - t_j)) * alpha.col(j) + piecewise_polynomial_part(t) - * - * @tparam_double_only - */ -template -class ExponentialPlusPiecewisePolynomial final - : public PiecewiseTrajectory { - public: - // We are final, so this is okay. - DRAKE_DEFAULT_COPY_AND_MOVE_AND_ASSIGN(ExponentialPlusPiecewisePolynomial) - - ExponentialPlusPiecewisePolynomial() = default; - - template - ExponentialPlusPiecewisePolynomial( - const Eigen::MatrixBase& K, - const Eigen::MatrixBase& A, - const Eigen::MatrixBase& alpha, - const PiecewisePolynomial& piecewise_polynomial_part) - : PiecewiseTrajectory(piecewise_polynomial_part), - K_(K), - A_(A), - alpha_(alpha), - piecewise_polynomial_part_(piecewise_polynomial_part) { - MALIPUT_DRAKE_ASSERT(K.rows() == rows()); - MALIPUT_DRAKE_ASSERT(K.cols() == A.rows()); - MALIPUT_DRAKE_ASSERT(A.rows() == A.cols()); - MALIPUT_DRAKE_ASSERT(alpha.rows() == A.cols()); - MALIPUT_DRAKE_ASSERT(alpha.cols() == - piecewise_polynomial_part.get_number_of_segments()); - MALIPUT_DRAKE_ASSERT(piecewise_polynomial_part.rows() == rows()); - MALIPUT_DRAKE_ASSERT(piecewise_polynomial_part.cols() == 1); - } - - // from PiecewisePolynomial - ExponentialPlusPiecewisePolynomial( - const PiecewisePolynomial& piecewise_polynomial_part); - - ~ExponentialPlusPiecewisePolynomial() override = default; - - std::unique_ptr> Clone() const override; - - MatrixX value(const T& t) const override; - - ExponentialPlusPiecewisePolynomial derivative(int derivative_order = 1) const; - - Eigen::Index rows() const override; - - Eigen::Index cols() const override; - - void shiftRight(double offset); - - private: - std::unique_ptr> DoMakeDerivative( - int derivative_order = 1) const override { - return derivative(derivative_order).Clone(); - }; - - MatrixX K_; - MatrixX A_; - MatrixX alpha_; - PiecewisePolynomial piecewise_polynomial_part_; -}; - -} // namespace trajectories -} // namespace maliput::drake diff --git a/include/maliput/drake/common/trajectories/piecewise_polynomial.h b/include/maliput/drake/common/trajectories/piecewise_polynomial.h index b212f5a..3f40725 100644 --- a/include/maliput/drake/common/trajectories/piecewise_polynomial.h +++ b/include/maliput/drake/common/trajectories/piecewise_polynomial.h @@ -1,5 +1,7 @@ #pragma once +#define MALIPUT_USED + #include #include #include diff --git a/include/maliput/drake/common/trajectories/piecewise_pose.h b/include/maliput/drake/common/trajectories/piecewise_pose.h deleted file mode 100644 index 9fb0616..0000000 --- a/include/maliput/drake/common/trajectories/piecewise_pose.h +++ /dev/null @@ -1,121 +0,0 @@ -#pragma once - -#include -#include - -#include "maliput/drake/common/drake_copyable.h" -#include "maliput/drake/common/eigen_types.h" -#include "maliput/drake/common/trajectories/piecewise_polynomial.h" -#include "maliput/drake/common/trajectories/piecewise_quaternion.h" -#include "maliput/drake/common/trajectories/piecewise_trajectory.h" -#include "maliput/drake/math/rigid_transform.h" - -namespace maliput::drake { -namespace trajectories { - -/** - * A wrapper class that represents a pose trajectory, whose rotation part is a - * PiecewiseQuaternionSlerp and the translation part is a PiecewisePolynomial. - * - * @tparam_default_scalars - */ -template -class PiecewisePose final : public PiecewiseTrajectory { - public: - DRAKE_DEFAULT_COPY_AND_MOVE_AND_ASSIGN(PiecewisePose) - - /** - * Constructs an empty piecewise pose trajectory. - */ - PiecewisePose() {} - - /** - * Constructor. - * @param pos_traj Position trajectory. - * @param rot_traj Orientation trajectory. - */ - PiecewisePose(const PiecewisePolynomial& pos_traj, - const PiecewiseQuaternionSlerp& rot_traj); - - /** - * Constructs a PiecewisePose from given @p time and @p poses. - * A cubic polynomial with given end velocities is used to construct the - * position part. The rotational part is represented by a piecewise quaterion - * trajectory. There must be at least two elements in @p times and @p poses. - * @param times Breaks used to build the splines. - * @param poses Knots used to build the splines. - * @param start_vel Start linear velocity. - * @param end_vel End linear velocity. - */ - static PiecewisePose MakeCubicLinearWithEndLinearVelocity( - const std::vector& times, - const std::vector>& poses, - const Vector3& start_vel = Vector3::Zero(), - const Vector3& end_vel = Vector3::Zero()); - - std::unique_ptr> Clone() const override; - - Eigen::Index rows() const override { return 4; } - - Eigen::Index cols() const override { return 4; } - - /** - * Returns the interpolated pose at @p time. - */ - math::RigidTransform get_pose(const T& time) const; - - MatrixX value(const T& t) const override { - return get_pose(t).GetAsMatrix4(); - } - - /** - * Returns the interpolated velocity at @p time or zero if @p time is before - * this trajectory's start time or after its end time. - */ - Vector6 get_velocity(const T& time) const; - - /** - * Returns the interpolated acceleration at @p time or zero if @p time is - * before this trajectory's start time or after its end time. - */ - Vector6 get_acceleration(const T& time) const; - - /** - * Returns true if the position and orientation trajectories are both - * within @p tol from the other's. - */ - bool is_approx(const PiecewisePose& other, double tol) const; - - /** - * Returns the position trajectory. - */ - const PiecewisePolynomial& get_position_trajectory() const { - return position_; - } - - /** - * Returns the orientation trajectory. - */ - const PiecewiseQuaternionSlerp& get_orientation_trajectory() const { - return orientation_; - } - - private: - bool do_has_derivative() const override; - - MatrixX DoEvalDerivative(const T& t, int derivative_order) const override; - - std::unique_ptr> DoMakeDerivative( - int derivative_order) const override; - - PiecewisePolynomial position_; - PiecewisePolynomial velocity_; - PiecewisePolynomial acceleration_; - PiecewiseQuaternionSlerp orientation_; -}; - -} // namespace trajectories -} // namespace maliput::drake - -DRAKE_DECLARE_CLASS_TEMPLATE_INSTANTIATIONS_ON_DEFAULT_SCALARS( - class maliput::drake::trajectories::PiecewisePose) diff --git a/include/maliput/drake/common/trajectories/piecewise_quaternion.h b/include/maliput/drake/common/trajectories/piecewise_quaternion.h deleted file mode 100644 index 9da6852..0000000 --- a/include/maliput/drake/common/trajectories/piecewise_quaternion.h +++ /dev/null @@ -1,174 +0,0 @@ -#pragma once - -#include -#include - -#include "maliput/drake/common/default_scalars.h" -#include "maliput/drake/common/drake_copyable.h" -#include "maliput/drake/common/eigen_types.h" -#include "maliput/drake/common/trajectories/piecewise_trajectory.h" -#include "maliput/drake/math/rotation_matrix.h" - -namespace maliput::drake { -namespace trajectories { - -/** - * A class representing a trajectory for quaternions that are interpolated - * using piecewise slerp (spherical linear interpolation). - * All the orientation samples are expected to be with respect to the same - * parent reference frame, i.e. q_i represents the rotation R_PBi for the - * orientation of frame B at the ith sample in a fixed parent frame P. - * The world frame is a common choice for the parent frame. - * The angular velocity and acceleration are also relative to the parent frame - * and expressed in the parent frame. - * Since there is a sign ambiguity when using quaternions to represent - * orientation, namely q and -q represent the same orientation, the internal - * quaternion representations ensure that q_n.dot(q_{n+1}) >= 0. - * Another intuitive way to think about this is that consecutive quaternions - * have the shortest geodesic distance on the unit sphere. - * - * @tparam_default_scalars - */ -template -class PiecewiseQuaternionSlerp final : public PiecewiseTrajectory { - public: - DRAKE_DEFAULT_COPY_AND_MOVE_AND_ASSIGN(PiecewiseQuaternionSlerp) - - /** - * Builds an empty PiecewiseQuaternionSlerp. - */ - PiecewiseQuaternionSlerp() = default; - - /** - * Builds a PiecewiseQuaternionSlerp. - * @throws std::exception if breaks and quaternions have different length, - * or breaks have length < 2. - */ - PiecewiseQuaternionSlerp( - const std::vector& breaks, - const std::vector>& quaternions); - - /** - * Builds a PiecewiseQuaternionSlerp. - * @throws std::exception if breaks and rot_matrices have different length, - * or breaks have length < 2. - */ - PiecewiseQuaternionSlerp( - const std::vector& breaks, - const std::vector>& rotation_matrices); - - /** - * Builds a PiecewiseQuaternionSlerp. - * @throws std::exception if breaks and rot_matrices have different length, - * or breaks have length < 2. - */ - PiecewiseQuaternionSlerp( - const std::vector& breaks, - const std::vector>& rotation_matrices); - - /** - * Builds a PiecewiseQuaternionSlerp. - * @throws std::exception if breaks and ang_axes have different length, - * or breaks have length < 2. - */ - PiecewiseQuaternionSlerp( - const std::vector& breaks, - const std::vector>& angle_axes); - - ~PiecewiseQuaternionSlerp() override = default; - - std::unique_ptr> Clone() const override; - - Eigen::Index rows() const override { return 4; } - - Eigen::Index cols() const override { return 1; } - - /** - * Interpolates orientation. - * @param time Time for interpolation. - * @return The interpolated quaternion at `time`. - */ - Quaternion orientation(const T& time) const; - - MatrixX value(const T& time) const override { - return orientation(time).matrix(); - } - - /** - * Interpolates angular velocity. - * @param time Time for interpolation. - * @return The interpolated angular velocity at `time`, - * which is constant per segment. - */ - Vector3 angular_velocity(const T& time) const; - - /** - * Interpolates angular acceleration. - * @param time Time for interpolation. - * @return The interpolated angular acceleration at `time`, - * which is always zero for slerp. - */ - Vector3 angular_acceleration(const T& time) const; - - /** - * Getter for the internal quaternion samples. - * - * @note The returned quaternions might be different from the ones used for - * construction because the internal representations are set to always be - * the "closest" w.r.t to the previous one. - * - * @return the internal sample points. - */ - const std::vector>& get_quaternion_samples() const { - return quaternions_; - } - - /** - * Returns true if all the corresponding segment times are within - * @p tol seconds, and the angle difference between the corresponding - * quaternion sample points are within @p tol (using `ExtractDoubleOrThrow`). - */ - bool is_approx(const PiecewiseQuaternionSlerp& other, - double tol) const; - - /** - * Given a new Quaternion, this method adds one segment to the end of `this`. - */ - void Append(const T& time, const Quaternion& quaternion); - - /** - * Given a new RotationMatrix, this method adds one segment to the end of - * `this`. - */ - void Append(const T& time, const math::RotationMatrix& rotation_matrix); - - /** - * Given a new AngleAxis, this method adds one segment to the end of `this`. - */ - void Append(const T& time, const AngleAxis& angle_axis); - - private: - // Initialize quaternions_ and computes angular velocity for each segment. - void Initialize( - const std::vector& breaks, - const std::vector>& quaternions); - - // Computes the interpolation time within each segment. Result is in [0, 1]. - T ComputeInterpTime(int segment_index, const T& time) const; - - bool do_has_derivative() const override; - - MatrixX DoEvalDerivative(const T& t, int derivative_order) const override; - - std::unique_ptr> DoMakeDerivative( - int derivative_order) const override; - - std::vector> quaternions_; - std::vector> angular_velocities_; -}; - -} // namespace trajectories -} // namespace maliput::drake - -DRAKE_DECLARE_CLASS_TEMPLATE_INSTANTIATIONS_ON_DEFAULT_SCALARS( - class maliput::drake::trajectories::PiecewiseQuaternionSlerp) diff --git a/include/maliput/drake/common/unused.h b/include/maliput/drake/common/unused.h index 1b9f9ec..7ccfe84 100644 --- a/include/maliput/drake/common/unused.h +++ b/include/maliput/drake/common/unused.h @@ -1,5 +1,7 @@ #pragma once +#define MALIPUT_USED + namespace maliput::drake { /// Documents the argument(s) as unused, placating GCC's -Wunused-parameter diff --git a/include/maliput/drake/common/value.h b/include/maliput/drake/common/value.h index 6df4988..520100a 100644 --- a/include/maliput/drake/common/value.h +++ b/include/maliput/drake/common/value.h @@ -1,5 +1,7 @@ #pragma once +#define MALIPUT_USED + #include #include #include diff --git a/include/maliput/drake/math/autodiff.h b/include/maliput/drake/math/autodiff.h deleted file mode 100644 index 009a96b..0000000 --- a/include/maliput/drake/math/autodiff.h +++ /dev/null @@ -1,328 +0,0 @@ -/// @file -/// Utilities for arithmetic on AutoDiffScalar. - -// TODO(russt): rename methods to be GSG compliant. - -#pragma once - -#include -#include - -#include - -#include "maliput/drake/common/autodiff.h" -#include "maliput/drake/common/unused.h" - -namespace maliput::drake { -namespace math { - -template -struct AutoDiffToValueMatrix { - typedef typename Eigen::Matrix type; -}; - -template -typename AutoDiffToValueMatrix::type autoDiffToValueMatrix( - const Eigen::MatrixBase& auto_diff_matrix) { - typename AutoDiffToValueMatrix::type ret(auto_diff_matrix.rows(), - auto_diff_matrix.cols()); - for (int i = 0; i < auto_diff_matrix.rows(); i++) { - for (int j = 0; j < auto_diff_matrix.cols(); ++j) { - ret(i, j) = auto_diff_matrix(i, j).value(); - } - } - return ret; -} - -/** `B = DiscardGradient(A)` enables casting from a matrix of AutoDiffScalars - * to AutoDiffScalar::Scalar type, explicitly throwing away any gradient - * information. For a matrix of type, e.g. `MatrixX A`, the - * comparable operation - * `B = A.cast()` - * should (and does) fail to compile. Use `DiscardGradient(A)` if you want to - * force the cast (and explicitly declare that information is lost). - * - * This method is overloaded to permit the user to call it for double types and - * AutoDiffScalar types (to avoid the calling function having to handle the - * two cases differently). - * - * @see DiscardZeroGradient - */ -template -typename std::enable_if_t< - !std::is_same_v, - Eigen::Matrix> -DiscardGradient(const Eigen::MatrixBase& auto_diff_matrix) { - return autoDiffToValueMatrix(auto_diff_matrix); -} - -/// @see DiscardGradient(). -template -typename std::enable_if_t< - std::is_same_v, - const Eigen::MatrixBase&> -DiscardGradient(const Eigen::MatrixBase& matrix) { - return matrix; -} - -/// @see DiscardGradient(). -template -typename std::enable_if_t< - !std::is_same_v<_Scalar, double>, - Eigen::Transform> -DiscardGradient(const Eigen::Transform<_Scalar, _Dim, _Mode, _Options>& - auto_diff_transform) { - return Eigen::Transform( - autoDiffToValueMatrix(auto_diff_transform.matrix())); -} - -/// @see DiscardGradient(). -template -typename std::enable_if_t, - const Eigen::Transform<_Scalar, _Dim, _Mode, - _Options>&> -DiscardGradient( - const Eigen::Transform<_Scalar, _Dim, _Mode, _Options>& transform) { - return transform; -} - - -/** \brief Initialize a single autodiff matrix given the corresponding value - *matrix. - * - * Set the values of \p auto_diff_matrix to be equal to \p val, and for each - *element i of \p auto_diff_matrix, - * resize the derivatives vector to \p num_derivatives, and set derivative - *number \p deriv_num_start + i to one (all other elements of the derivative - *vector set to zero). - * - * \param[in] mat 'regular' matrix of values - * \param[out] ret AutoDiff matrix - * \param[in] num_derivatives the size of the derivatives vector @default the - *size of mat - * \param[in] deriv_num_start starting index into derivative vector (i.e. - *element deriv_num_start in derivative vector corresponds to mat(0, 0)). - *@default 0 - */ -template -void initializeAutoDiff(const Eigen::MatrixBase& val, - // TODO(#2274) Fix NOLINTNEXTLINE(runtime/references). - Eigen::MatrixBase& auto_diff_matrix, - Eigen::DenseIndex num_derivatives = Eigen::Dynamic, - Eigen::DenseIndex deriv_num_start = 0) { - using ADScalar = typename DerivedAutoDiff::Scalar; - static_assert(static_cast(Derived::RowsAtCompileTime) == - static_cast(DerivedAutoDiff::RowsAtCompileTime), - "auto diff matrix has wrong number of rows at compile time"); - static_assert(static_cast(Derived::ColsAtCompileTime) == - static_cast(DerivedAutoDiff::ColsAtCompileTime), - "auto diff matrix has wrong number of columns at compile time"); - - if (num_derivatives == Eigen::Dynamic) num_derivatives = val.size(); - - auto_diff_matrix.resize(val.rows(), val.cols()); - Eigen::DenseIndex deriv_num = deriv_num_start; - for (Eigen::DenseIndex i = 0; i < val.size(); i++) { - auto_diff_matrix(i) = ADScalar(val(i), num_derivatives, deriv_num++); - } -} - -/** \brief The appropriate AutoDiffScalar gradient type given the value type and - * the number of derivatives at compile time - */ -template -using AutoDiffMatrixType = Eigen::Matrix< - Eigen::AutoDiffScalar>, - Derived::RowsAtCompileTime, Derived::ColsAtCompileTime, 0, - Derived::MaxRowsAtCompileTime, Derived::MaxColsAtCompileTime>; - -/** \brief Initialize a single autodiff matrix given the corresponding value - *matrix. - * - * Create autodiff matrix that matches \p mat in size with derivatives of - *compile time size \p Nq and runtime size \p num_derivatives. - * Set its values to be equal to \p val, and for each element i of \p - *auto_diff_matrix, set derivative number \p deriv_num_start + i to one (all - *other derivatives set to zero). - * - * \param[in] mat 'regular' matrix of values - * \param[in] num_derivatives the size of the derivatives vector @default the - *size of mat - * \param[in] deriv_num_start starting index into derivative vector (i.e. - *element deriv_num_start in derivative vector corresponds to mat(0, 0)). - *@default 0 - * \return AutoDiff matrix - */ -template -AutoDiffMatrixType initializeAutoDiff( - const Eigen::MatrixBase& mat, - Eigen::DenseIndex num_derivatives = -1, - Eigen::DenseIndex deriv_num_start = 0) { - if (num_derivatives == -1) num_derivatives = mat.size(); - - AutoDiffMatrixType ret(mat.rows(), mat.cols()); - initializeAutoDiff(mat, ret, num_derivatives, deriv_num_start); - return ret; -} - -namespace internal { -template -struct ResizeDerivativesToMatchScalarImpl { - // TODO(#2274) Fix NOLINTNEXTLINE(runtime/references). - static void run(Eigen::MatrixBase&, const Scalar&) {} -}; - -template -struct ResizeDerivativesToMatchScalarImpl> { - using Scalar = Eigen::AutoDiffScalar; - // TODO(#2274) Fix NOLINTNEXTLINE(runtime/references). - static void run(Eigen::MatrixBase& mat, const Scalar& scalar) { - for (int i = 0; i < mat.size(); i++) { - auto& derivs = mat(i).derivatives(); - if (derivs.size() == 0) { - derivs.resize(scalar.derivatives().size()); - derivs.setZero(); - } - } - } -}; -} // namespace internal - -/** Resize derivatives vector of each element of a matrix to match the size - * of the derivatives vector of a given scalar. - * \brief If the mat and scalar inputs are AutoDiffScalars, resize the - * derivatives vector of each element of the matrix mat to match - * the number of derivatives of the scalar. This is useful in functions that - * return matrices that do not depend on an AutoDiffScalar - * argument (e.g. a function with a constant output), while it is desired that - * information about the number of derivatives is preserved. - * \param mat matrix, for which the derivative vectors of the elements will be - * resized - * \param scalar scalar to match the derivative size vector against. - */ -template -// TODO(#2274) Fix NOLINTNEXTLINE(runtime/references). -void resizeDerivativesToMatchScalar(Eigen::MatrixBase& mat, - const typename Derived::Scalar& scalar) { - internal::ResizeDerivativesToMatchScalarImpl< - Derived, typename Derived::Scalar>::run(mat, scalar); -} - -namespace internal { -/* Helper for totalSizeAtCompileTime function (recursive) */ -template -struct TotalSizeAtCompileTime { - static constexpr int eval() { - return Head::SizeAtCompileTime == Eigen::Dynamic || - TotalSizeAtCompileTime::eval() == Eigen::Dynamic - ? Eigen::Dynamic - : Head::SizeAtCompileTime + - TotalSizeAtCompileTime::eval(); - } -}; - -/* Helper for totalSizeAtCompileTime function (base case) */ -template -struct TotalSizeAtCompileTime { - static constexpr int eval() { return Head::SizeAtCompileTime; } -}; - -/* Determine the total size at compile time of a number of arguments - * based on their SizeAtCompileTime static members - */ -template -constexpr int totalSizeAtCompileTime() { - return TotalSizeAtCompileTime::eval(); -} - -/* Determine the total size at runtime of a number of arguments using - * their size() methods (base case). - */ -constexpr Eigen::DenseIndex totalSizeAtRunTime() { return 0; } - -/* Determine the total size at runtime of a number of arguments using - * their size() methods (recursive) - */ -template -Eigen::DenseIndex totalSizeAtRunTime(const Eigen::MatrixBase& head, - const Tail&... tail) { - return head.size() + totalSizeAtRunTime(tail...); -} - -/* Helper for initializeAutoDiffTuple function (recursive) */ -template -struct InitializeAutoDiffTupleHelper { - template - static void run(const std::tuple& values, - // TODO(#2274) Fix NOLINTNEXTLINE(runtime/references). - std::tuple& auto_diffs, - Eigen::DenseIndex num_derivatives, - Eigen::DenseIndex deriv_num_start) { - constexpr size_t tuple_index = sizeof...(AutoDiffTypes)-Index; - const auto& value = std::get(values); - auto& auto_diff = std::get(auto_diffs); - auto_diff.resize(value.rows(), value.cols()); - initializeAutoDiff(value, auto_diff, num_derivatives, deriv_num_start); - InitializeAutoDiffTupleHelper::run( - values, auto_diffs, num_derivatives, deriv_num_start + value.size()); - } -}; - -/* Helper for initializeAutoDiffTuple function (base case) */ -template <> -struct InitializeAutoDiffTupleHelper<0> { - template - static void run(const std::tuple& values, - const std::tuple& auto_diffs, - Eigen::DenseIndex num_derivatives, - Eigen::DenseIndex deriv_num_start) { - unused(values, auto_diffs, num_derivatives, deriv_num_start); - } -}; -} // namespace internal - -/** \brief Given a series of Eigen matrices, create a tuple of corresponding - *AutoDiff matrices with values equal to the input matrices and properly - *initialized derivative vectors. - * - * The size of the derivative vector of each element of the matrices in the - *output tuple will be the same, and will equal the sum of the number of - *elements of the matrices in \p args. - * If all of the matrices in \p args have fixed size, then the derivative - *vectors will also have fixed size (being the sum of the sizes at compile time - *of all of the input arguments), - * otherwise the derivative vectors will have dynamic size. - * The 0th element of the derivative vectors will correspond to the derivative - *with respect to the 0th element of the first argument. - * Subsequent derivative vector elements correspond first to subsequent elements - *of the first input argument (traversed first by row, then by column), and so - *on for subsequent arguments. - * - * \param args a series of Eigen matrices - * \return a tuple of properly initialized AutoDiff matrices corresponding to \p - *args - * - */ -template -std::tuple()>...> -initializeAutoDiffTuple(const Eigen::MatrixBase&... args) { - Eigen::DenseIndex dynamic_num_derivs = internal::totalSizeAtRunTime(args...); - std::tuple()>...> - ret(AutoDiffMatrixType()>( - args.rows(), args.cols())...); - auto values = std::forward_as_tuple(args...); - internal::InitializeAutoDiffTupleHelper::run( - values, ret, dynamic_num_derivs, 0); - return ret; -} - -} // namespace math -} // namespace maliput::drake diff --git a/include/maliput/drake/math/autodiff_gradient.h b/include/maliput/drake/math/autodiff_gradient.h deleted file mode 100644 index 3d213a7..0000000 --- a/include/maliput/drake/math/autodiff_gradient.h +++ /dev/null @@ -1,214 +0,0 @@ -/// @file -/// Utilities that relate simultaneously to both autodiff matrices and -/// gradient matrices. - -#pragma once - -#include - -#include - -#include "maliput/drake/common/drake_assert.h" -#include "maliput/drake/common/unused.h" -#include "maliput/drake/math/autodiff.h" -#include "maliput/drake/math/gradient.h" - -namespace maliput::drake { -namespace math { - -template -struct AutoDiffToGradientMatrix { - typedef typename Gradient< - Eigen::Matrix, - Eigen::Dynamic>::type type; -}; - -template -typename AutoDiffToGradientMatrix::type autoDiffToGradientMatrix( - const Eigen::MatrixBase& auto_diff_matrix, - int num_variables = Eigen::Dynamic) { - int num_variables_from_matrix = 0; - for (int i = 0; i < auto_diff_matrix.size(); ++i) { - num_variables_from_matrix = - std::max(num_variables_from_matrix, - static_cast(auto_diff_matrix(i).derivatives().size())); - } - if (num_variables == Eigen::Dynamic) { - num_variables = num_variables_from_matrix; - } else if (num_variables_from_matrix != 0 && - num_variables_from_matrix != num_variables) { - std::stringstream buf; - buf << "Input matrix has derivatives w.r.t " << num_variables_from_matrix - << " variables, whereas num_variables is " << num_variables << ".\n"; - buf << "Either num_variables_from_matrix should be zero, or it should " - "match num_variables."; - throw std::runtime_error(buf.str()); - } - - typename AutoDiffToGradientMatrix::type gradient( - auto_diff_matrix.size(), num_variables); - for (int row = 0; row < auto_diff_matrix.rows(); row++) { - for (int col = 0; col < auto_diff_matrix.cols(); col++) { - auto gradient_row = - gradient.row(row + col * auto_diff_matrix.rows()).transpose(); - if (auto_diff_matrix(row, col).derivatives().size() == 0) { - gradient_row.setZero(); - } else { - gradient_row = auto_diff_matrix(row, col).derivatives(); - } - } - } - return gradient; -} - -/** \brief Initializes an autodiff matrix given a matrix of values and gradient - * matrix - * \param[in] val value matrix - * \param[in] gradient gradient matrix; the derivatives of val(j) are stored in - * row j of the gradient matrix. - * \param[out] autodiff_matrix matrix of AutoDiffScalars with the same size as - * \p val - */ -template -void initializeAutoDiffGivenGradientMatrix( - const Eigen::MatrixBase& val, - const Eigen::MatrixBase& gradient, - // TODO(#2274) Fix NOLINTNEXTLINE(runtime/references). - Eigen::MatrixBase& auto_diff_matrix) { - static_assert(static_cast(Derived::SizeAtCompileTime) == - static_cast(DerivedGradient::RowsAtCompileTime), - "gradient has wrong number of rows at compile time"); - MALIPUT_DRAKE_ASSERT(val.size() == gradient.rows() && - "gradient has wrong number of rows at runtime"); - typedef AutoDiffMatrixType - ExpectedAutoDiffType; - static_assert(static_cast(ExpectedAutoDiffType::RowsAtCompileTime) == - static_cast(DerivedAutoDiff::RowsAtCompileTime), - "auto diff matrix has wrong number of rows at compile time"); - static_assert(static_cast(ExpectedAutoDiffType::ColsAtCompileTime) == - static_cast(DerivedAutoDiff::ColsAtCompileTime), - "auto diff matrix has wrong number of columns at compile time"); - static_assert(std::is_same_v, - "wrong auto diff scalar type"); - - typedef typename Eigen::MatrixBase::Index Index; - auto_diff_matrix.resize(val.rows(), val.cols()); - auto num_derivs = gradient.cols(); - for (Index row = 0; row < auto_diff_matrix.size(); row++) { - auto_diff_matrix(row).value() = val(row); - auto_diff_matrix(row).derivatives().resize(num_derivs, 1); - auto_diff_matrix(row).derivatives() = gradient.row(row).transpose(); - } -} - -/** \brief Creates and initializes an autodiff matrix given a matrix of values - * and gradient matrix - * \param[in] val value matrix - * \param[in] gradient gradient matrix; the derivatives of val(j) are stored in - * row j of the gradient matrix. - * \return autodiff_matrix matrix of AutoDiffScalars with the same size as \p - * val - */ -template -AutoDiffMatrixType -initializeAutoDiffGivenGradientMatrix( - const Eigen::MatrixBase& val, - const Eigen::MatrixBase& gradient) { - AutoDiffMatrixType ret( - val.rows(), val.cols()); - initializeAutoDiffGivenGradientMatrix(val, gradient, ret); - return ret; -} - -template -void gradientMatrixToAutoDiff( - const Eigen::MatrixBase& gradient, - // TODO(#2274) Fix NOLINTNEXTLINE(runtime/references). - Eigen::MatrixBase& auto_diff_matrix) { - typedef typename Eigen::MatrixBase::Index Index; - auto nx = gradient.cols(); - for (Index row = 0; row < auto_diff_matrix.rows(); row++) { - for (Index col = 0; col < auto_diff_matrix.cols(); col++) { - auto_diff_matrix(row, col).derivatives().resize(nx, 1); - auto_diff_matrix(row, col).derivatives() = - gradient.row(row + col * auto_diff_matrix.rows()).transpose(); - } - } -} - -/** `B = DiscardZeroGradient(A, precision)` enables casting from a matrix of - * AutoDiffScalars to AutoDiffScalar::Scalar type, but first checking that - * the gradient matrix is empty or zero. For a matrix of type, e.g. - * `MatrixX A`, the comparable operation - * `B = A.cast()` - * should (and does) fail to compile. Use `DiscardZeroGradient(A)` if you want - * to force the cast (and the check). - * - * This method is overloaded to permit the user to call it for double types and - * AutoDiffScalar types (to avoid the calling function having to handle the - * two cases differently). - * - * @param precision is passed to Eigen's isZero(precision) to evaluate whether - * the gradients are zero. - * @throws std::exception if the gradients were not empty nor zero. - * - * @see DiscardGradient - */ -template -typename std::enable_if_t< - !std::is_same_v, - Eigen::Matrix> -DiscardZeroGradient( - const Eigen::MatrixBase& auto_diff_matrix, - const typename Eigen::NumTraits< - typename Derived::Scalar::Scalar>::Real& precision = - Eigen::NumTraits::dummy_precision()) { - const auto gradients = autoDiffToGradientMatrix(auto_diff_matrix); - if (gradients.size() == 0 || gradients.isZero(precision)) { - return autoDiffToValueMatrix(auto_diff_matrix); - } - throw std::runtime_error( - "Casting AutoDiff to value but gradients are not zero."); -} - -/// @see DiscardZeroGradient(). -template -typename std::enable_if_t, - const Eigen::MatrixBase&> -DiscardZeroGradient(const Eigen::MatrixBase& matrix, - double precision = 0.) { - unused(precision); - return matrix; -} - -/// @see DiscardZeroGradient(). -template -typename std::enable_if_t< - !std::is_same_v<_Scalar, double>, - Eigen::Transform> -DiscardZeroGradient( - const Eigen::Transform<_Scalar, _Dim, _Mode, _Options>& auto_diff_transform, - const typename Eigen::NumTraits::Real& precision = - Eigen::NumTraits::dummy_precision()) { - return Eigen::Transform( - DiscardZeroGradient(auto_diff_transform.matrix(), precision)); -} - -/// @see DiscardZeroGradient(). -template -typename std::enable_if_t< - std::is_same_v<_Scalar, double>, - const Eigen::Transform<_Scalar, _Dim, _Mode, _Options>&> -DiscardZeroGradient( - const Eigen::Transform<_Scalar, _Dim, _Mode, _Options>& transform, - double precision = 0.) { - unused(precision); - return transform; -} - -} // namespace math -} // namespace maliput::drake diff --git a/include/maliput/drake/math/barycentric.h b/include/maliput/drake/math/barycentric.h deleted file mode 100644 index b8ea1ac..0000000 --- a/include/maliput/drake/math/barycentric.h +++ /dev/null @@ -1,181 +0,0 @@ -#pragma once - -#include -#include -#include -#include - -#include - -#include "maliput/drake/common/drake_assert.h" -#include "maliput/drake/common/drake_copyable.h" -#include "maliput/drake/common/eigen_types.h" - -namespace maliput::drake { -namespace math { - -/// Represents a multi-linear function (from vector inputs to vector outputs) by -/// interpolating between points on a mesh using (triangular) barycentric -/// interpolation. -/// -/// For a technical description of barycentric interpolation, see e.g. -/// Remi Munos and Andrew Moore, "Barycentric Interpolators for Continuous -/// Space and Time Reinforcement Learning", NIPS 1998 -/// -/// @tparam_double_only -template -class BarycentricMesh { - // TODO(russt): This is also an instance of a "linear function approximator" - // -- a class of parameterized functions that take the form - // output = parameters*φ(input), - // where - // parameters is a matrix of size num_outputs x num_features, with column i - // the vector value of ith mesh point. - // φ(input) is a vector of length num_features, where element i is the - // interpolation coefficient of the ith mesh point. - // Only num_interpolants of these features are non-zero in any query. - // - // If we implement more function approximators, then I'm tempted to - // call the base classes e.g. ParameterizedFunction and - // LinearlyParameterizedFunction. - // - // Note: here we have a matrix of parameters and a feature vector, when the - // more typical case of linear function approximators would use a vector of - // parameters and a feature matrix. - - public: - DRAKE_DEFAULT_COPY_AND_MOVE_AND_ASSIGN(BarycentricMesh); - - /// The mesh is represented by a std::set (to ensure uniqueness and provide - /// logarithmic lookups) of coordinates in each input dimension. Note: The - /// values are type double, not T (We do not plan to take gradients, etc w/ - /// respect to them). - typedef std::set Coordinates; - typedef std::vector MeshGrid; - - /// Constructs the mesh. - explicit BarycentricMesh(MeshGrid input_grid); - - // Accessor methods. - const MeshGrid& get_input_grid() const { return input_grid_; } - int get_input_size() const { return input_grid_.size(); } - int get_num_mesh_points() const { - int num_mesh_points = 1; - for (const auto& coords : input_grid_) { - num_mesh_points *= coords.size(); - } - return num_mesh_points; - } - int get_num_interpolants() const { return num_interpolants_; } - - /// Writes the position of a mesh point in the input space referenced by its - /// scalar index to @p point. - /// @param index must be in [0, get_num_mesh_points). - /// @param point is set to the num_inputs-by-1 location of the mesh point. - void get_mesh_point(int index, EigenPtr point) const; - - /// Returns the position of a mesh point in the input space referenced by its - /// scalar index to @p point. - /// @param index must be in [0, get_num_mesh_points). - VectorX get_mesh_point(int index) const; - - /// Returns a matrix with all of the mesh points, one per column. - MatrixX get_all_mesh_points() const; - - /// Writes the mesh indices used for interpolation to @p mesh_indices, and the - /// interpolating coefficients to @p weights. Inputs that are outside the - /// bounding box of the input_grid are interpolated as though they were - /// projected (elementwise) to the closest face of the defined mesh. - /// - /// @param input must be a vector of length get_num_inputs(). - /// @param mesh_indices is a pointer to a vector of length - /// get_num_interpolants(). - /// @param weights is a vector of coefficients (which sum to 1) of length - /// get_num_interpolants(). - void EvalBarycentricWeights(const Eigen::Ref>& input, - EigenPtr mesh_indices, - EigenPtr> weights) const; - - /// Evaluates the function at the @p input values, by interpolating between - /// the values at @p mesh_values. Inputs that are outside the - /// bounding box of the input_grid are interpolated as though they were - /// projected (elementwise) to the closest face of the defined mesh. - /// - /// Note that the dimension of the output vector is completely defined by the - /// mesh_values argument. This class does not maintain any information - /// related to the size of the output. - /// - /// @param mesh_values is a num_outputs by get_num_mesh_points() matrix - /// containing the points to interpolate between. The order of the columns - /// must be consistent with the mesh indices curated by this class, as exposed - /// by get_mesh_point(). - /// @param input must be a vector of length get_num_inputs(). - /// @param output is the interpolated vector of length num_outputs - void Eval(const Eigen::Ref>& mesh_values, - const Eigen::Ref>& input, - EigenPtr> output) const; - - /// Returns the function evaluated at @p input. - VectorX Eval(const Eigen::Ref>& mesh_values, - const Eigen::Ref>& input) const; - - /// Performs Eval, but with the possibility of the values on the mesh - /// having a different scalar type than the values defining the mesh - /// (symbolic::Expression containing decision variables for an optimization - /// problem is an important example) - /// @tparam ValueT defines the scalar type of the mesh_values and the output. - /// @see Eval - template - void EvalWithMixedScalars( - const Eigen::Ref>& mesh_values, - const Eigen::Ref>& input, - EigenPtr> output) const { - MALIPUT_DRAKE_DEMAND(output != nullptr); - MALIPUT_DRAKE_DEMAND(input.size() == get_input_size()); - MALIPUT_DRAKE_DEMAND(mesh_values.cols() == get_num_mesh_points()); - - Eigen::VectorXi mesh_indices(num_interpolants_); - VectorX weights(num_interpolants_); - - EvalBarycentricWeights(input, &mesh_indices, &weights); - - *output = weights[0] * mesh_values.col(mesh_indices[0]); - for (int i = 1; i < num_interpolants_; i++) { - *output += weights[i] * mesh_values.col(mesh_indices[i]); - } - } - - /// Returns the function evaluated at @p input. - template - VectorX EvalWithMixedScalars( - const Eigen::Ref>& mesh_values, - const Eigen::Ref>& input) const { - VectorX output(mesh_values.rows()); - EvalWithMixedScalars(mesh_values, input, &output); - return output; - } - - /// Evaluates @p vector_func at all input mesh points and extracts the mesh - /// value matrix that should be used to approximate the function with this - /// barycentric interpolation. - /// - /// @code - /// MatrixXd mesh_values = bary.MeshValuesFrom( - /// [](const auto& x) { return Vector1d(std::sin(x[0])); }); - /// @endcode - MatrixX MeshValuesFrom( - const std::function(const Eigen::Ref>&)>& - vector_func) const; - - private: - MeshGrid input_grid_; // Specifies the location of the mesh points in - // the input space. - std::vector stride_; // The number of elements to skip to arrive at the - // next value (per input dimension) - int num_interpolants_{1}; // The number of points used in any interpolation. -}; - -} // namespace math -} // namespace maliput::drake - -extern template class ::maliput::drake::math::BarycentricMesh; diff --git a/include/maliput/drake/math/bspline_basis.h b/include/maliput/drake/math/bspline_basis.h deleted file mode 100644 index e55f5a7..0000000 --- a/include/maliput/drake/math/bspline_basis.h +++ /dev/null @@ -1,213 +0,0 @@ -#pragma once - -#include -#include - -#include "maliput/drake/common/drake_assert.h" -#include "maliput/drake/common/drake_bool.h" -#include "maliput/drake/common/drake_copyable.h" -#include "maliput/drake/common/drake_throw.h" -#include "maliput/drake/common/name_value.h" -#include "maliput/drake/math/knot_vector_type.h" - -namespace maliput::drake { -namespace math { -/** Given a set of non-descending breakpoints t₀ ≤ t₁ ≤ ⋅⋅⋅ ≤ tₘ, a B-spline -basis of order k is a set of n + 1 (where n = m - k) piecewise polynomials of -degree k - 1 defined over those breakpoints. The elements of this set are -called "B-splines". The vector (t₀, t₁, ..., tₘ)' is referred to as -the "knot vector" of the basis and its elements are referred to as "knots". - -At a breakpoint with multiplicity p (i.e. a breakpoint that appears p times in -the knot vector), B-splines are guaranteed to have Cᵏ⁻ᵖ⁻¹ continuity. - -A B-spline curve using a B-spline basis B, is a parametric curve mapping -parameter values in [tₖ₋₁, tₙ₊₁] to a vector space V. For t ∈ [tₖ₋₁, tₙ₊₁] the -value of the curve is given by the linear combination of n + 1 control points, -pᵢ ∈ V, with the elements of B evaluated at t. - -For more information on B-splines and their uses, see (for example) -Patrikalakis et al. [1]. - -[1] https://web.mit.edu/hyperbook/Patrikalakis-Maekawa-Cho/node15.html */ -template -class BsplineBasis final { - public: - DRAKE_DEFAULT_COPY_AND_MOVE_AND_ASSIGN(BsplineBasis); - - BsplineBasis() : BsplineBasis(0, {}) {} - - /** Constructs a B-spline basis with the specified `order` and `knots`. - @pre `knots` is sorted in non-descending order. - @throws std::exception if knots.size() < 2 * order. */ - BsplineBasis(int order, std::vector knots); - - /** Constructs a B-spline basis with the specified `order`, - `num_basis_functions`, `initial_parameter_value`, `final_parameter_value`, - and an auto-generated knot vector of the specified `type`. - @throws std::exception if num_basis_functions < order - @pre initial_parameter_value ≤ final_parameter_value */ - BsplineBasis(int order, int num_basis_functions, - KnotVectorType type = KnotVectorType::kClampedUniform, - const T& initial_parameter_value = 0, - const T& final_parameter_value = 1); - -#ifdef DRAKE_DOXYGEN_CXX - /** Conversion constructor. Constructs an instance of BsplineBasis from a - double-valued basis. */ - explicit BsplineBasis(const BsplineBasis& other); -#else - template - explicit BsplineBasis(const BsplineBasis& other, - /* Prevents ambiguous declarations between default copy - constructor on double and conversion constructor on T = double. - The conversion constructor for T = double will fail to be - instantiated because the second, "hidden" parameter will fail to - be defined for U = double. */ - typename std::enable_if_t>* = {}) - : order_(other.order()) { - knots_.reserve(other.knots().size()); - for (const auto& knot : other.knots()) { - knots_.push_back(T(knot)); - } - } -#endif - - /** The order of this B-spline basis (k in the class description). */ - int order() const { return order_; } - - /** The degree of the piecewise polynomials comprising this B-spline basis - (k - 1 in the class description). */ - int degree() const { return order() - 1; } - - /** The number of basis functions in this B-spline basis (n + 1 in the class - description). */ - int num_basis_functions() const { return knots_.size() - order_; } - - /** The knot vector of this B-spline basis (the vector (t₀, t₁, ..., tₘ)' in - the class description). */ - const std::vector& knots() const { return knots_; } - - /** The minimum allowable parameter value for B-spline curves using this - basis (tₖ₋₁ in the class description). */ - const T& initial_parameter_value() const { return knots()[order() - 1]; } - - /** The maximum allowable parameter value for B-spline curves using this - basis (tₙ₊₁ in the class description). */ - const T& final_parameter_value() const { - return knots()[num_basis_functions()]; - } - - /** For a `parameter_value` = t, the interval that contains it is the pair of - knot values [tᵢ, tᵢ₊₁] for the greatest i such that tᵢ ≤ t and - tᵢ < final_parameter_value(). This function returns that value of i. - @pre parameter_value ≥ initial_parameter_value() - @pre parameter_value ≤ final_parameter_value() */ - int FindContainingInterval(const T& parameter_value) const; - - /** Returns the indices of the basis functions which may evaluate to non-zero - values for some parameter value in `parameter_interval`; all other basis - functions are strictly zero over `parameter_interval`. - @pre parameter_interval[0] ≤ parameter_interval[1] - @pre parameter_interval[0] ≥ initial_parameter_value() - @pre parameter_interval[1] ≤ final_parameter_value() */ - std::vector ComputeActiveBasisFunctionIndices( - const std::array& parameter_interval) const; - - /** Returns the indices of the basis functions which may evaluate to non-zero - values for `parameter_value`; all other basis functions are strictly zero at - this point. - @pre parameter_value ≥ initial_parameter_value() - @pre parameter_value ≤ final_parameter_value() */ - std::vector ComputeActiveBasisFunctionIndices( - const T& parameter_value) const; - - /** Evaluates the B-spline curve defined by `this` and `control_points` at the - given `parameter_value`. - @param control_points Control points of the B-spline curve. - @param parameter_value Parameter value at which to evaluate the B-spline - curve defined by `this` and `control_points`. - @pre control_points.size() == num_basis_functions() - @pre parameter_value ≥ initial_parameter_value() - @pre parameter_value ≤ final_parameter_value() */ - template - T_control_point EvaluateCurve( - const std::vector& control_points, - const T& parameter_value) const { - /* This function implements the de Boor algorithm. It uses the notation - from Patrikalakis et al. [1]. Since the depth of recursion is known - a-priori, the algorithm is flattened along the lines described in [2] to - avoid duplicate computations. - - [1] https://web.mit.edu/hyperbook/Patrikalakis-Maekawa-Cho/node18.html - [2] De Boor, Carl. "On calculating with B-splines." Journal of - Approximation theory 6.1 (1972): 50-62. - - NOTE: The implementation of this method is included in the header so that - it can be used with custom values of T_control_point. */ - MALIPUT_DRAKE_DEMAND(static_cast(control_points.size()) == - num_basis_functions()); - MALIPUT_DRAKE_DEMAND(parameter_value >= initial_parameter_value()); - MALIPUT_DRAKE_DEMAND(parameter_value <= final_parameter_value()); - - // Define short names to match notation in [1]. - const std::vector& t = knots(); - const T& t_bar = parameter_value; - const int k = order(); - - /* Find the index, 𝑙, of the greatest knot that is less than or equal to - t_bar and strictly less than final_parameter_value(). */ - const int ell = FindContainingInterval(t_bar); - // The vector that stores the intermediate de Boor points (the pᵢʲ in [1]). - std::vector p(order()); - /* For j = 0, i goes from ell down to ell - (k - 1). Define r such that - i = ell - r. */ - for (int r = 0; r < k; ++r) { - const int i = ell - r; - p.at(r) = control_points.at(i); - } - /* For j = 1, ..., k - 1, i goes from ell down to ell - (k - j - 1). Again, - i = ell - r. */ - for (int j = 1; j < k; ++j) { - for (int r = 0; r < k - j; ++r) { - const int i = ell - r; - // α = (t_bar - t[i]) / (t[i + k - j] - t[i]); - const T alpha = (t_bar - t.at(i)) / (t.at(i + k - j) - t.at(i)); - p.at(r) = (1.0 - alpha) * p.at(r + 1) + alpha * p.at(r); - } - } - return p.front(); - } - - /** Returns the value of the `i`-th basis function evaluated at - `parameter_value`. */ - T EvaluateBasisFunctionI(int i, const T& parameter_value) const; - - boolean operator==(const BsplineBasis& other) const; - - boolean operator!=(const BsplineBasis& other) const; - - /** Passes this object to an Archive; see @ref serialize_tips for background. - This method is only available when T = double. */ - template -#ifdef DRAKE_DOXYGEN_CXX - void -#else - // Restrict this method to T = double only; we must mix "Archive" into the - // conditional type for SFINAE to work, so we just check it against void. - std::enable_if_t && !std::is_void_v> -#endif - Serialize(Archive* a) { - a->Visit(MakeNameValue("order", &order_)); - a->Visit(MakeNameValue("knots", &knots_)); - MALIPUT_DRAKE_THROW_UNLESS(CheckInvariants()); - } - - private: - bool CheckInvariants() const; - - int order_{}; - std::vector knots_; -}; -} // namespace math -} // namespace maliput::drake diff --git a/include/maliput/drake/math/compute_numerical_gradient.h b/include/maliput/drake/math/compute_numerical_gradient.h deleted file mode 100644 index f2e4408..0000000 --- a/include/maliput/drake/math/compute_numerical_gradient.h +++ /dev/null @@ -1,178 +0,0 @@ -#pragma once - -#include - -#include "maliput/drake/common/eigen_types.h" - -namespace maliput::drake { -namespace math { - -enum class NumericalGradientMethod { - kForward, ///< Compute the gradient as (f(x + Δx) - f(x)) / Δx, with Δx > 0 - kBackward, ///< Compute the gradient as (f(x) - f(x - Δx)) / Δx, with Δx > 0 - kCentral, ///< Compute the gradient as (f(x + Δx) - f(x - Δx)) / (2Δx), with - ///< Δx > 0 -}; - -class NumericalGradientOption { - public: - /** - * @param function_accuracy The accuracy of evaluating function f(x). For - * double-valued functions (with magnitude around 1), the accuracy is usually - * about 1E-15. - */ - explicit NumericalGradientOption(NumericalGradientMethod method, - double function_accuracy = 1e-15) - : method_{method} { - perturbation_size_ = method == NumericalGradientMethod::kCentral - ? std::cbrt(function_accuracy) - : std::sqrt(function_accuracy); - } - - NumericalGradientMethod method() const { return method_; } - - double perturbation_size() const { return perturbation_size_; } - - private: - NumericalGradientMethod method_{NumericalGradientMethod::kCentral}; - - /** The step length Δx is max(|x(i)| * perturbation_size, perturbation_size) - * in each dimension. If function f is evaluated with accuracy 10⁻¹⁵, then - * a first-order method (forward or backward difference) should use a - * perturbation of √10⁻¹⁵ ≈ 10⁻⁷, a second-order method (central - * difference) should use ∛10⁻¹⁵≈ 10⁻⁵. The interested reader could refer - * to section 8.6 of Practical Optimization by Philip E. Gill, Walter Murray - * and Margaret H. Wright. - */ - double perturbation_size_{NAN}; -}; - -/** - * Compute the gradient of a function f(x) through numerical difference. - * @param calc_fun calc_fun(x, &y) computes the value of f(x), and stores the - * value in y. `calc_fun` is responsible for properly resizing the output `y` - * when it consists of an Eigen vector of Eigen::Dynamic size. - * - * @param x The point at which the numerical gradient is computed. - * @param option The options for computing numerical gradient. - * @tparam DerivedX an Eigen column vector. - * @tparam DerivedY an Eigen column vector. - * @tparam DerivedCalcX The type of x in the calc_fun. Must be an Eigen column - * vector. It is possible to have DerivedCalcX being different from - * DerivedX, for example, `calc_fun` could be solvers::EvaluatorBase(const - * Eigen::Ref&, Eigen::VectorXd*), but `x` could be of - * type Eigen::VectorXd. - * TODO(hongkai.dai): understand why the default template DerivedCalcX = - * DerivedX doesn't compile when I instantiate - * ComputeNumericalGradient(calc_fun, x); - * @retval gradient a matrix of size x.rows() x y.rows(). gradient(i, j) is - * ∂f(i) / ∂x(j) - * - * Examples: - * @code{cc} - * // Create a std::function from a lambda expression. - * std::function foo = [](const - * Eigen::Vector2d& x, Vector3d*y) { (*y)(0) = x(0); (*y)(1) = x(0) * x(1); - * (*y)(2) = x(0) * std::sin(x(1));}; - * Eigen::Vector3d x_eval(1, 2, 3); - * auto J = ComputeNumericalGradient(foo, x_eval); - * // Note that if we pass in a lambda to ComputeNumericalGradient, then - * // ComputeNumericalGradient has to instantiate the template types explicitly, - * // as in this example. The issue of template deduction with std::function is - * // explained in - * // - * https://stackoverflow.com/questions/48529410/template-arguments-deduction-failed-passing-func-pointer-to-stdfunction - * auto bar = [](const Eigen::Vector2d& x, Eigen::Vector2d* y) {*y = x; }; - * auto J2 = ComputeNumericalGradient(bar, Eigen::Vector2d(2, 3)); - * - * @endcode - */ -template -Eigen::Matrix -ComputeNumericalGradient( - std::function calc_fun, - const DerivedX& x, - const NumericalGradientOption& option = NumericalGradientOption{ - NumericalGradientMethod::kForward}) { - // All input Eigen types must be vectors templated on the same scalar type. - typedef typename DerivedX::Scalar T; - static_assert( - is_eigen_vector_of::value, - "DerivedY must be templated on the same scalar type as DerivedX."); - static_assert( - is_eigen_vector_of::value, - "DerivedCalcX must be templated on the same scalar type as DerivedX."); - - using std::abs; - using std::max; - - // Notation: - // We will always approximate the derivative as: - // J.ⱼ = (y⁺ - y⁻) / dxⱼ - // where J.ⱼ is the j-th column of J. We define y⁺ = y(x⁺) and y⁻ = y(x⁻) - // depending on the scheme as: - // x⁺ = x , for backward differences. - // = x + h eⱼ, for forward and central differences. - // x⁻ = x , for forward differences. - // = x - h eⱼ, for backward and central differences. - // where eⱼ is the standard basis in ℝⁿ and h is the perturbation size. - // Finally, dxⱼ = x⁺ⱼ - x⁻ⱼ. - - // N.B. We do not know the size of y at this point. We'll know it after the - // first function evaluation. - Eigen::Matrix y_plus, y_minus; - - // We need to evaluate f(x), only for the forward and backward schemes. - if (option.method() == NumericalGradientMethod::kBackward) { - calc_fun(x, &y_plus); - } else if (option.method() == NumericalGradientMethod::kForward) { - calc_fun(x, &y_minus); - } - - // Now evaluate f(x + Δx) and f(x - Δx) along each dimension. - Eigen::Matrix x_plus = x; - Eigen::Matrix x_minus = x; - Eigen::Matrix J; - for (int i = 0; i < x.rows(); ++i) { - // Perturbation size. - const T h = - max(option.perturbation_size(), abs(x(i)) * option.perturbation_size()); - - if (option.method() == NumericalGradientMethod::kForward || - option.method() == NumericalGradientMethod::kCentral) { - x_plus(i) += h; - calc_fun(x_plus, &y_plus); - } - - if (option.method() == NumericalGradientMethod::kBackward || - option.method() == NumericalGradientMethod::kCentral) { - x_minus(i) -= h; - calc_fun(x_minus, &y_minus); - } - - // Update dxi, minimizing the effect of roundoff error by ensuring that - // x⁺ and x⁻ differ by an exactly representable number. See p. 192 of - // Press, W., Teukolsky, S., Vetterling, W., and Flannery, P. Numerical - // Recipes in C++, 2nd Ed., Cambridge University Press, 2002. - // In addition, notice that dxi = 2 * h for central - // differences. - const T dxi = x_plus(i) - x_minus(i); - - // Resize if we haven't yet. - // N.B. The size of y is known not until the first function evaluation. - // Therefore we delay the resizing of J to this point. - MALIPUT_DRAKE_ASSERT(y_minus.size() == y_plus.size()); - MALIPUT_DRAKE_ASSERT(x_minus.size() == x_plus.size()); - if (J.size() == 0) J.resize(y_minus.size(), x_minus.size()); - - J.col(i) = (y_plus - y_minus) / dxi; - - // Restore perturbed values. - x_plus(i) = x_minus(i) = x(i); - } - return J; -} -} // namespace math -} // namespace maliput::drake diff --git a/include/maliput/drake/math/continuous_algebraic_riccati_equation.h b/include/maliput/drake/math/continuous_algebraic_riccati_equation.h deleted file mode 100644 index c9bac0f..0000000 --- a/include/maliput/drake/math/continuous_algebraic_riccati_equation.h +++ /dev/null @@ -1,36 +0,0 @@ -#pragma once - -#include - -namespace maliput::drake { -namespace math { - -/// Computes the unique stabilizing solution S to the continuous-time algebraic -/// Riccati equation: -/// -/// @f[ -/// S A + A' S - S B R^{-1} B' S + Q = 0 -/// @f] -/// -/// @throws std::exception if R is not positive definite. -/// -/// Based on the Matrix Sign Function method outlined in this paper: -/// http://www.engr.iupui.edu/~skoskie/ECE684/Riccati_algorithms.pdf -/// -Eigen::MatrixXd ContinuousAlgebraicRiccatiEquation( - const Eigen::Ref& A, - const Eigen::Ref& B, - const Eigen::Ref& Q, - const Eigen::Ref& R); - -/// This is functionally the same as -/// ContinuousAlgebraicRiccatiEquation(A, B, Q, R). -/// The Cholesky decomposition of R is passed in instead of R. -Eigen::MatrixXd ContinuousAlgebraicRiccatiEquation( - const Eigen::Ref& A, - const Eigen::Ref& B, - const Eigen::Ref& Q, - const Eigen::LLT& R_cholesky); - -} // namespace math -} // namespace maliput::drake diff --git a/include/maliput/drake/math/continuous_lyapunov_equation.h b/include/maliput/drake/math/continuous_lyapunov_equation.h deleted file mode 100644 index e218e76..0000000 --- a/include/maliput/drake/math/continuous_lyapunov_equation.h +++ /dev/null @@ -1,80 +0,0 @@ -#pragma once - -#include -#include - -#include "maliput/drake/common/eigen_types.h" - -namespace maliput::drake { -namespace math { - -// TODO(FischerGundlach) Reserve memory and pass it to recursive function -// calls. - -/** - * @param A A user defined real square matrix. - * @param Q A user defined real symmetric matrix. - * - * @pre Q is a symmetric matrix. - * - * Computes a unique solution X to the continuous Lyapunov equation: `AᵀX + XA + - * Q = 0`, where A is real and square, and Q is real, symmetric and of equal - * size as A. - * @throws std::exception if A or Q are not square matrices or do not - * have the same size. - * - * Limitations: Given the Eigenvalues of A as λ₁, ..., λₙ, there exists - * a unique solution if and only if λᵢ + λ̅ⱼ ≠ 0 ∀ i,j, where λ̅ⱼ is - * the complex conjugate of λⱼ. - * @throws std::exception if the solution is not unique. - * - * There are no further limitations on the eigenvalues of A. - * Further, if all λᵢ have negative real parts, and if Q is positive - * semi-definite, then X is also positive semi-definite [1]. Therefore, if one - * searches for a Lyapunov function V(z) = zᵀXz for the stable linear system - * ż = Az, then the solution of the Lyapunov Equation `AᵀX + XA + Q = 0` only - * returns a valid Lyapunov function if Q is positive semi-definite. - * - * The implementation is based on SLICOT routine SB03MD [2]. Note the - * transformation Q = -C. The complexity of this routine is O(n³). - * If A is larger than 2-by-2, then a Schur factorization is performed. - * @throws std::exception if Schur factorization failed. - * - * A tolerance of ε is used to check if a double variable is equal to zero, - * where the default value for ε is 1e-10. It has been used to check (1) if λᵢ + - * λ̅ⱼ = 0, ∀ i,j; (2) if A is a 1-by-1 zero matrix; (3) if A's trace or - * determinant is 0 when A is a 2-by-2 matrix. - * - * [1] Bartels, R.H. and G.W. Stewart, "Solution of the Matrix Equation AX + XB - * = C," Comm. of the ACM, Vol. 15, No. 9, 1972. - * - * [2] http://slicot.org/objects/software/shared/doc/SB03MD.html - * - */ - -Eigen::MatrixXd RealContinuousLyapunovEquation( - const Eigen::Ref& A, - const Eigen::Ref& Q); - -namespace internal { - -// Subroutines which help special cases. These cases are also called within -// SolveReducedRealContinuousLyapunovFunction. - -Vector1d Solve1By1RealContinuousLyapunovEquation( - const Eigen::Ref& A, const Eigen::Ref& Q); - -Eigen::Matrix2d Solve2By2RealContinuousLyapunovEquation( - const Eigen::Ref& A, - const Eigen::Ref& Q); - -// If the problem size is larger than 2-by-2, then it is reduced into a form -// which can be recursively solved by smaller problems. - -Eigen::MatrixXd SolveReducedRealContinuousLyapunovEquation( - const Eigen::Ref& A, - const Eigen::Ref& Q); - -} // namespace internal -} // namespace math -} // namespace maliput::drake diff --git a/include/maliput/drake/math/convert_time_derivative.h b/include/maliput/drake/math/convert_time_derivative.h deleted file mode 100644 index e7a5c72..0000000 --- a/include/maliput/drake/math/convert_time_derivative.h +++ /dev/null @@ -1,52 +0,0 @@ -#pragma once - -#include - -#include "maliput/drake/common/eigen_types.h" - -namespace maliput::drake { -namespace math { - -/// Given ᴮd/dt(v) (the time derivative in frame B of an arbitrary 3D vector v) -/// and given ᴬωᴮ (frame B's angular velocity in another frame A), this method -/// computes ᴬd/dt(v) (the time derivative in frame A of v) by: -/// ᴬd/dt(v) = ᴮd/dt(v) + ᴬωᴮ x v -/// -/// This mathematical operation is known as the "Transport Theorem" or the -/// "Golden Rule for Vector Differentiation" [Mitiguy 2016, §7.3]. It was -/// discovered by Euler in 1758. Its explicit notation with superscript -/// frames was invented by Thomas Kane in 1950. Its use as the defining -/// property of angular velocity was by Mitiguy in 1993. -/// -/// In source code and comments, we use the following monogram notations: -/// DtA_v = ᴬd/dt(v) denotes the time derivative in frame A of the vector v. -/// DtA_v_E = [ᴬd/dt(v)]_E denotes the time derivative in frame A of vector v, -/// with the resulting new vector quantity expressed in a frame E. -/// -/// In source code, this mathematical operation is performed with all vectors -/// expressed in the same frame E as [ᴬd/dt(v)]ₑ = [ᴮd/dt(v)]ₑ + [ᴬωᴮ]ₑ x [v]ₑ -/// which in monogram notation is: 
-///   DtA_v_E = DtB_v_E + w_AB_E x v_E
-///
-/// -/// [Mitiguy 2016] Mitiguy, P., 2016. Advanced Dynamics & Motion Simulation. -template -Vector3 ConvertTimeDerivativeToOtherFrame( - const Eigen::MatrixBase& v_E, - const Eigen::MatrixBase& DtB_v_E, - const Eigen::MatrixBase& w_AB_E) { - // All input vectors must be three dimensional vectors. - EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(Eigen::MatrixBase, 3); - EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(Eigen::MatrixBase, 3); - EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(Eigen::MatrixBase, 3); - typedef typename v_Type::Scalar T; - // All input vectors must be templated on the same scalar type. - static_assert(std::is_same_v, - "DtB_v_E must be templated on the same scalar type as v_E"); - static_assert(std::is_same_v, - "w_AB_E must be templated on the same scalar type as v_E"); - return DtB_v_E + w_AB_E.cross(v_E); -} - -} // namespace math -} // namespace maliput::drake diff --git a/include/maliput/drake/math/cross_product.h b/include/maliput/drake/math/cross_product.h deleted file mode 100644 index 3113a0b..0000000 --- a/include/maliput/drake/math/cross_product.h +++ /dev/null @@ -1,21 +0,0 @@ -#pragma once - -#include - -#include "maliput/drake/common/constants.h" -#include "maliput/drake/common/eigen_types.h" - -namespace maliput::drake { -namespace math { -template -drake::Matrix3 VectorToSkewSymmetric( - const Eigen::MatrixBase& p) { - EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(Eigen::MatrixBase, - maliput::drake::kSpaceDimension); - maliput::drake::Matrix3 ret; - ret << 0.0, -p(2), p(1), p(2), 0.0, -p(0), -p(1), p(0), 0.0; - return ret; -} - -} // namespace math -} // namespace maliput::drake diff --git a/include/maliput/drake/math/discrete_algebraic_riccati_equation.h b/include/maliput/drake/math/discrete_algebraic_riccati_equation.h deleted file mode 100644 index f1b3cef..0000000 --- a/include/maliput/drake/math/discrete_algebraic_riccati_equation.h +++ /dev/null @@ -1,80 +0,0 @@ -#pragma once - -#include -#include - -#include - -namespace maliput::drake { -namespace math { - -/// Computes the unique stabilizing solution X to the discrete-time algebraic -/// Riccati equation: -/// -/// AᵀXA − X − AᵀXB(BᵀXB + R)⁻¹BᵀXA + Q = 0 -/// -/// @throws std::exception if Q is not positive semi-definite. -/// @throws std::exception if R is not positive definite. -/// -/// Based on the Schur Vector approach outlined in this paper: -/// "On the Numerical Solution of the Discrete-Time Algebraic Riccati Equation" -/// by Thrasyvoulos Pappas, Alan J. Laub, and Nils R. Sandell -/// -Eigen::MatrixXd DiscreteAlgebraicRiccatiEquation( - const Eigen::Ref& A, - const Eigen::Ref& B, - const Eigen::Ref& Q, - const Eigen::Ref& R); - -/// Computes the unique stabilizing solution X to the discrete-time algebraic -/// Riccati equation: -/// -/// AᵀXA − X − (AᵀXB + N)(BᵀXB + R)⁻¹(BᵀXA + Nᵀ) + Q = 0 -/// -/// This is equivalent to solving the original DARE: -/// -/// A₂ᵀXA₂ − X − A₂ᵀXB(BᵀXB + R)⁻¹BᵀXA₂ + Q₂ = 0 -/// -/// where A₂ and Q₂ are a change of variables: -/// -/// A₂ = A − BR⁻¹Nᵀ and Q₂ = Q − NR⁻¹Nᵀ -/// -/// This overload of the DARE is useful for finding the control law uₖ that -/// minimizes the following cost function subject to xₖ₊₁ = Axₖ + Buₖ. -/// -/// @verbatim -/// ∞ [xₖ]ᵀ[Q N][xₖ] -/// J = Σ [uₖ] [Nᵀ R][uₖ] ΔT -/// k=0 -/// @endverbatim -/// -/// This is a more general form of the following. The linear-quadratic regulator -/// is the feedback control law uₖ that minimizes the following cost function -/// subject to xₖ₊₁ = Axₖ + Buₖ: -/// -/// @verbatim -/// ∞ -/// J = Σ (xₖᵀQxₖ + uₖᵀRuₖ) ΔT -/// k=0 -/// @endverbatim -/// -/// This can be refactored as: -/// -/// @verbatim -/// ∞ [xₖ]ᵀ[Q 0][xₖ] -/// J = Σ [uₖ] [0 R][uₖ] ΔT -/// k=0 -/// @endverbatim -/// -/// @throws std::runtime_error if Q − NR⁻¹Nᵀ is not positive semi-definite. -/// @throws std::runtime_error if R is not positive definite. -/// -Eigen::MatrixXd DiscreteAlgebraicRiccatiEquation( - const Eigen::Ref& A, - const Eigen::Ref& B, - const Eigen::Ref& Q, - const Eigen::Ref& R, - const Eigen::Ref& N); -} // namespace math -} // namespace maliput::drake - diff --git a/include/maliput/drake/math/discrete_lyapunov_equation.h b/include/maliput/drake/math/discrete_lyapunov_equation.h deleted file mode 100644 index 060c5e9..0000000 --- a/include/maliput/drake/math/discrete_lyapunov_equation.h +++ /dev/null @@ -1,81 +0,0 @@ -#pragma once - -#include -#include - -#include "maliput/drake/common/eigen_types.h" - -namespace maliput::drake { -namespace math { - -// TODO(FischerGundlach) Reserve memory and pass it to recursive function -// calls. - -/** - * @param A A user defined real square matrix. - * @param Q A user defined real symmetric matrix. - * - * @pre Q is a symmetric matrix. - * - * Computes the unique solution X to the discrete Lyapunov equation: `AᵀXA - X + - * Q = 0`, where A is real and square, and Q is real, symmetric and of equal - * size as A. - * @throws std::exception if A or Q are not square matrices or do not - * have the same size. - * - * Limitations: Given the Eigenvalues of A as λ₁, ..., λₙ, there exists - * a unique solution if and only if λᵢ * λⱼ ≠ 1 ∀ i,j and λᵢ ≠ ±1, ∀ i [1]. - * @throws std::exception if the solution is not unique.[3] - * - * There are no further limitations on the eigenvalues of A. - * Further, if |λᵢ|<1, ∀ i, and if Q is - * positive semi-definite, then X is also positive semi-definite [2]. - * Therefore, if one searches for a Lyapunov function V(z) = zᵀXz for the stable - * linear system zₙ₊₁ = Azₙ, then the solution of the Lyapunov Equation `AᵀXA - - * X + Q = 0` only returns a valid Lyapunov function if Q is positive - * semi-definite. - * - * The implementation is based on SLICOT routine SB03MD [2]. Note the - * transformation Q = -C. The complexity of this routine is O(n³). - * If A is larger than 2-by-2, then a Schur factorization is performed. - * @throws std::exception if Schur factorization fails. - * - * A tolerance of ε is used to check if a double variable is equal to zero, - * where the default value for ε is 1e-10. It has been used to check (1) if λᵢ = - * ±1 ∀ i; (2) if λᵢ * λⱼ = 1, i ≠ j. - * - * [1] Barraud, A.Y., "A numerical algorithm to solve AᵀXA - X = Q," IEEE® - * Trans. Auto. Contr., AC-22, pp. 883-885, 1977. - * - * [2] http://slicot.org/objects/software/shared/doc/SB03MD.html - * - * [3] https://www.mathworks.com/help/control/ref/dlyap.html - * - */ - -Eigen::MatrixXd RealDiscreteLyapunovEquation( - const Eigen::Ref& A, - const Eigen::Ref& Q); - -namespace internal { - -// Subroutines which help special cases. These cases are also called within -// SolveReducedRealContinuousLyapunovFunction. - -Vector1d Solve1By1RealDiscreteLyapunovEquation( - const Eigen::Ref& A, const Eigen::Ref& Q); - -Eigen::Matrix2d Solve2By2RealDiscreteLyapunovEquation( - const Eigen::Ref& A, - const Eigen::Ref& Q); - -// If the problem is larger than in size 2-by-2, than it is reduced into a form -// which can be recursively solved by smaller problems. - -Eigen::MatrixXd SolveReducedRealDiscreteLyapunovEquation( - const Eigen::Ref& A, - const Eigen::Ref& Q); - -} // namespace internal -} // namespace math -} // namespace maliput::drake diff --git a/include/maliput/drake/math/eigen_sparse_triplet.h b/include/maliput/drake/math/eigen_sparse_triplet.h deleted file mode 100644 index 9a9f89b..0000000 --- a/include/maliput/drake/math/eigen_sparse_triplet.h +++ /dev/null @@ -1,85 +0,0 @@ -#pragma once - -#include - -#include - -namespace maliput::drake { -namespace math { -/** - * For a sparse matrix, return a vector of triplets, such that we can - * reconstruct the matrix using setFromTriplet function - * @param matrix A sparse matrix - * @return A triplet with the row, column and value of the non-zero entries. - * See https://eigen.tuxfamily.org/dox/group__TutorialSparse.html for more - * information on the triplet - */ -template -std::vector> SparseMatrixToTriplets( - const Derived& matrix) { - using Scalar = typename Derived::Scalar; - std::vector> triplets; - triplets.reserve(matrix.nonZeros()); - for (int i = 0; i < matrix.outerSize(); i++) { - for (typename Derived::InnerIterator it(matrix, i); it; ++it) { - triplets.push_back( - Eigen::Triplet(it.row(), it.col(), it.value())); - } - } - return triplets; -} - -/** - * For a sparse matrix, return the row indices, the column indices, and value of - * the non-zero entries. - * For example, the matrix - * - * \f[ - * mat = \begin{bmatrix} 1 & 0 & 2\\ - * 0 & 3 & 4\end{bmatrix} - * \f] - * has - * \f[ - * row = \begin{bmatrix} 0 & 1 & 0 & 1\end{bmatrix}\\ - * col = \begin{bmatrix} 0 & 1 & 2 & 2\end{bmatrix}\\ - * val = \begin{bmatrix} 1 & 3 & 2 & 4\end{bmatrix} - * \f] - * @param[in] matrix the input sparse matrix - * @param[out] row_indices a vector containing the row indices of the - * non-zero entries - * @param[out] col_indices a vector containing the column indices of the - * non-zero entries - * @param[out] val a vector containing the values of the non-zero entries. - */ -template -void SparseMatrixToRowColumnValueVectors( - const Derived& matrix, - // TODO(#2274) Fix NOLINTNEXTLINE(runtime/references). - std::vector& row_indices, - // TODO(#2274) Fix NOLINTNEXTLINE(runtime/references). - std::vector& col_indices, - // TODO(#2274) Fix NOLINTNEXTLINE(runtime/references). - std::vector& val) { - row_indices.clear(); - col_indices.clear(); - val.clear(); - int nnz = matrix.nonZeros(); - row_indices.reserve(nnz); - col_indices.reserve(nnz); - val.reserve(nnz); - for (int i = 0; i < matrix.outerSize(); i++) { - for (typename Derived::InnerIterator it(matrix, i); it; ++it) { - row_indices.push_back(it.row()); - col_indices.push_back(it.col()); - val.push_back(it.value()); - } - } -} -} // namespace math -} // namespace maliput::drake diff --git a/include/maliput/drake/math/evenly_distributed_pts_on_sphere.h b/include/maliput/drake/math/evenly_distributed_pts_on_sphere.h deleted file mode 100644 index 433a56f..0000000 --- a/include/maliput/drake/math/evenly_distributed_pts_on_sphere.h +++ /dev/null @@ -1,19 +0,0 @@ -#pragma once -#include - -namespace maliput::drake { -namespace math { -/** - * Deterministically generates approximate evenly distributed points on a unit - * sphere. This method uses Fibonacci number. For the detailed math, please - * refer to - * http://stackoverflow.com/questions/9600801/evenly-distributing-n-points-on-a-sphere - * This algorithm generates the points in O(n) time, where `n` is the number of - * points. - * @param num_points The number of points we want on the unit sphere. - * @return The generated points. - * @pre num_samples >= 1. Throw std::exception if num_points < 1 - */ -Eigen::Matrix3Xd UniformPtsOnSphereFibonacci(int num_points); -} // namespace math -} // namespace maliput::drake diff --git a/include/maliput/drake/math/fast_pose_composition_functions.h b/include/maliput/drake/math/fast_pose_composition_functions.h deleted file mode 100644 index f1a3760..0000000 --- a/include/maliput/drake/math/fast_pose_composition_functions.h +++ /dev/null @@ -1,105 +0,0 @@ -#pragma once - -/** @file -Declarations for fast, low-level functions for handling objects stored in small -matrices with known memory layouts. Ideally these are implemented using -platform-specific SIMD instructions for speed; however, we always provide a -straightforward portable fallback. */ - -/* N.B. Do not include code from drake/common here because this file will be -included by a compilation unit that may have a different opinion about whether -SIMD instructions are enabled than Eigen does in the rest of Drake. */ - -namespace maliput::drake { -namespace math { - -/* We do not have access to the declarations for RotationMatrix and -RigidTransform here. Instead, the functions below depend on knowledge of the -internal storage format of these classes, which is guaranteed and enforced by -the class declarations: - - Drake RotationMatrix objects are stored as 3x3 column-ordered matrices in - nine consecutive doubles. - - Drake RigidTransform objects are stored as 3x4 column-ordered matrices in - twelve consecutive doubles. The first nine elements comprise a 3x3 - RotationMatrix and the last three are the translation vector. */ - -template -class RotationMatrix; -template -class RigidTransform; - -namespace internal { - -/* Composes two maliput::drake::math::RotationMatrix objects as quickly as -possible, resulting in a new RotationMatrix. - -Here we calculate `R_AC = R_AB * R_BC`. It is OK for R_AC to overlap -with one or both inputs. */ -void ComposeRR(const RotationMatrix& R_AB, - const RotationMatrix& R_BC, - RotationMatrix* R_AC); - -/* Composes the inverse of a maliput::drake::math::RotationMatrix object with -another (non-inverted) maliput::drake::math::RotationMatrix as quickly as -possible, resulting in a new RotationMatrix. - -@note A valid RotationMatrix is orthonormal, and the inverse of an orthonormal -matrix is just its transpose. This function assumes orthonormality and hence -simply multiplies the transpose of its first argument by the second. - -Here we calculate `R_AC = R_BA⁻¹ * R_BC`. It is OK for R_AC to overlap -with one or both inputs. */ -void ComposeRinvR(const RotationMatrix& R_BA, - const RotationMatrix& R_BC, - RotationMatrix* R_AC); - -/** Composes two maliput::drake::math::RigidTransform objects as quickly as -possible, resulting in a new RigidTransform. - -@note This function is specialized for RigidTransforms and is not just a -matrix multiply. - -Here we calculate `X_AC = X_AB * X_BC`. It is OK for X_AC to overlap -with one or both inputs. */ -void ComposeXX(const RigidTransform& X_AB, - const RigidTransform& X_BC, - RigidTransform* X_AC); - -/** Composes the inverse of a maliput::drake::math::RigidTransform object with -another (non-inverted) maliput::drake::math::RigidTransform as quickly as -possible, resulting in a new RigidTransform. - -@note This function is specialized for RigidTransforms and is not just a -matrix multiply. - -Here we calculate `X_AC = X_BA⁻¹ * X_BC`. It is OK for X_AC to overlap -with one or both inputs. */ -void ComposeXinvX(const RigidTransform& X_BA, - const RigidTransform& X_BC, - RigidTransform* X_AC); - -/* Returns `true` if we are using the portable fallback implementations for -the above functions. */ -bool IsUsingPortableCompositionFunctions(); - -/* These portable implementations are exposed so they can be unit tested. -Call IsUsingPortableCompositionFunctions() to determine whether these are being -used to implement the above functions. */ - -void ComposeRRPortable(const RotationMatrix& R_AB, - const RotationMatrix& R_BC, - RotationMatrix* R_AC); -void ComposeRinvRPortable(const RotationMatrix& R_BA, - const RotationMatrix& R_BC, - RotationMatrix* R_AC); - -void ComposeXXPortable(const RigidTransform& X_AB, - const RigidTransform& X_BC, - RigidTransform* X_AC); -void ComposeXinvXPortable(const RigidTransform& X_BA, - const RigidTransform& X_BC, - RigidTransform* X_AC); - -} // namespace internal -} // namespace math -} // namespace maliput::drake diff --git a/include/maliput/drake/math/gradient.h b/include/maliput/drake/math/gradient.h deleted file mode 100644 index 8e50be4..0000000 --- a/include/maliput/drake/math/gradient.h +++ /dev/null @@ -1,36 +0,0 @@ -/// @file -/// Utilities for arithmetic on gradients. - -#pragma once - -#include - -namespace maliput::drake { -namespace math { - -/* - * Recursively defined template specifying a matrix type of the correct size for - * a gradient of a matrix function with respect to Nq variables, of any order. - */ -template -struct Gradient { - typedef typename Eigen::Matrix< - typename Derived::Scalar, - ((Derived::SizeAtCompileTime == Eigen::Dynamic || Nq == Eigen::Dynamic) - ? Eigen::Dynamic - : Gradient::type::SizeAtCompileTime), - Nq> type; -}; - -/* - * Base case for recursively defined gradient template. - */ -template -struct Gradient { - typedef typename Eigen::Matrix type; -}; - -} // namespace math -} // namespace maliput::drake diff --git a/include/maliput/drake/math/gradient_util.h b/include/maliput/drake/math/gradient_util.h deleted file mode 100644 index 10bdfc2..0000000 --- a/include/maliput/drake/math/gradient_util.h +++ /dev/null @@ -1,295 +0,0 @@ -#pragma once - -#include -#include - -#include -#include - -#include "maliput/drake/common/drake_assert.h" -#include "maliput/drake/math/gradient.h" - -namespace maliput::drake { -namespace math { - -template -std::array intRange(int start) { - std::array ret; - for (unsigned int i = 0; i < Size; i++) { - ret[i] = i + start; - } - return ret; -} - -/* - * Output type of matGradMultMat - */ -template -struct MatGradMultMat { - typedef typename Eigen::Matrix< - typename DerivedA::Scalar, - (DerivedA::RowsAtCompileTime == Eigen::Dynamic || - DerivedB::ColsAtCompileTime == Eigen::Dynamic - ? Eigen::Dynamic - : static_cast(DerivedA::RowsAtCompileTime) * - static_cast(DerivedB::ColsAtCompileTime)), - DerivedDA::ColsAtCompileTime> type; -}; - -/* - * Output type of matGradMult - */ -template -struct MatGradMult { - typedef typename Eigen::Matrix< - typename DerivedDA::Scalar, - (DerivedDA::RowsAtCompileTime == Eigen::Dynamic || - DerivedB::SizeAtCompileTime == Eigen::Dynamic - ? Eigen::Dynamic - : static_cast(DerivedDA::RowsAtCompileTime) / - static_cast(DerivedB::RowsAtCompileTime) * - static_cast(DerivedB::ColsAtCompileTime)), - DerivedDA::ColsAtCompileTime> type; -}; - -/* - * Output type and array types of getSubMatrixGradient for std::arrays - * specifying rows and columns - */ -template -struct GetSubMatrixGradientArray { - typedef typename Eigen::Matrix type; -}; - -/* - * Output type of getSubMatrixGradient for a single element of the input matrix - */ -template -struct GetSubMatrixGradientSingleElement { - typedef typename Eigen::Block type; -}; - -/* - * Profile results: looks like return value optimization works; a version that - * sets a reference - * instead of returning a value is just as fast and this is cleaner. - */ -template -typename Derived::PlainObject transposeGrad( - const Eigen::MatrixBase& dX, typename Derived::Index rows_X) { - typename Derived::PlainObject dX_transpose(dX.rows(), dX.cols()); - typename Derived::Index numel = dX.rows(); - typename Derived::Index index = 0; - for (int i = 0; i < numel; i++) { - dX_transpose.row(i) = dX.row(index); - index += rows_X; - if (index >= numel) { - index = (index % numel) + 1; - } - } - return dX_transpose; -} - -template -typename MatGradMultMat::type matGradMultMat( - const Eigen::MatrixBase& A, const Eigen::MatrixBase& B, - const Eigen::MatrixBase& dA, - const Eigen::MatrixBase& dB) { - MALIPUT_DRAKE_ASSERT(dA.cols() == dB.cols()); - - typename MatGradMultMat::type ret( - A.rows() * B.cols(), dA.cols()); - - for (int col = 0; col < B.cols(); col++) { - auto block = ret.template block( - col * A.rows(), 0, A.rows(), dA.cols()); - - // A * dB part: - block.noalias() = A * - dB.template block( - col * A.cols(), 0, A.cols(), dA.cols()); - - for (int row = 0; row < B.rows(); row++) { - // B * dA part: - block.noalias() += B(row, col) * - dA.template block( - row * A.rows(), 0, A.rows(), dA.cols()); - } - } - return ret; - - // much slower and requires eigen/unsupported: - // return Eigen::kroneckerProduct(Eigen::MatrixXd::Identity(B.cols(), - // B.cols()), A) * dB + Eigen::kroneckerProduct(B.transpose(), - // Eigen::MatrixXd::Identity(A.rows(), A.rows())) * dA; -} - -template -typename MatGradMult::type matGradMult( - const Eigen::MatrixBase& dA, - const Eigen::MatrixBase& B) { - MALIPUT_DRAKE_ASSERT(B.rows() == 0 ? dA.rows() == 0 : dA.rows() % B.rows() == 0); - typename DerivedDA::Index A_rows = B.rows() == 0 ? 0 : dA.rows() / B.rows(); - const int A_rows_at_compile_time = - (DerivedDA::RowsAtCompileTime == Eigen::Dynamic || - DerivedB::RowsAtCompileTime == Eigen::Dynamic) - ? Eigen::Dynamic - : static_cast(DerivedDA::RowsAtCompileTime) / - static_cast(DerivedB::RowsAtCompileTime); - - typename MatGradMult::type ret(A_rows * B.cols(), - dA.cols()); - ret.setZero(); - for (int col = 0; col < B.cols(); col++) { - auto block = ret.template block( - col * A_rows, 0, A_rows, dA.cols()); - for (int row = 0; row < B.rows(); row++) { - // B * dA part: - block.noalias() += B(row, col) * - dA.template block( - row * A_rows, 0, A_rows, dA.cols()); - } - } - return ret; -} - -// TODO(tkoolen): could save copies once -// http://eigen.tuxfamily.org/bz/show_bug.cgi?id=329 is fixed -template -Eigen::Matrix -getSubMatrixGradient(const Eigen::MatrixBase& dM, - const std::vector& rows, const std::vector& cols, - typename Derived::Index M_rows, int q_start = 0, - typename Derived::Index q_subvector_size = -1) { - if (q_subvector_size < 0) { - q_subvector_size = dM.cols() - q_start; - } - Eigen::MatrixXd dM_submatrix(rows.size() * cols.size(), q_subvector_size); - int index = 0; - for (std::vector::const_iterator col = cols.begin(); col != cols.end(); - ++col) { - for (std::vector::const_iterator row = rows.begin(); row != rows.end(); - ++row) { - dM_submatrix.row(index) = - dM.block(*row + *col * M_rows, q_start, 1, q_subvector_size); - index++; - } - } - return dM_submatrix; -} - -template -typename GetSubMatrixGradientArray::type -getSubMatrixGradient( - const Eigen::MatrixBase& dM, const std::array& rows, - const std::array& cols, typename Derived::Index M_rows, - int q_start = 0, - typename Derived::Index q_subvector_size = QSubvectorSize) { - if (q_subvector_size == Eigen::Dynamic) { - q_subvector_size = dM.cols() - q_start; - } - Eigen::Matrix dM_submatrix(NRows * NCols, - q_subvector_size); - int index = 0; - for (typename std::array::const_iterator col = cols.begin(); - col != cols.end(); ++col) { - for (typename std::array::const_iterator row = rows.begin(); - row != rows.end(); ++row) { - dM_submatrix.row(index++) = dM.template block<1, QSubvectorSize>( - (*row) + (*col) * M_rows, q_start, 1, q_subvector_size); - } - } - return dM_submatrix; -} - -template -typename GetSubMatrixGradientSingleElement::type -getSubMatrixGradient( - const Eigen::MatrixBase& dM, int row, int col, - typename Derived::Index M_rows, typename Derived::Index q_start = 0, - typename Derived::Index q_subvector_size = QSubvectorSize) { - if (q_subvector_size == Eigen::Dynamic) { - q_subvector_size = dM.cols() - q_start; - } - return dM.template block<1, QSubvectorSize>(row + col * M_rows, q_start, 1, - q_subvector_size); -} - -template -// TODO(#2274) Fix NOLINTNEXTLINE(runtime/references). -void setSubMatrixGradient(Eigen::MatrixBase& dM, - const Eigen::MatrixBase& dM_submatrix, - const std::vector& rows, - const std::vector& cols, - typename DerivedA::Index M_rows, - typename DerivedA::Index q_start = 0, - typename DerivedA::Index q_subvector_size = -1) { - if (q_subvector_size < 0) { - q_subvector_size = dM.cols() - q_start; - } - int index = 0; - for (typename std::vector::const_iterator col = cols.begin(); - col != cols.end(); ++col) { - for (typename std::vector::const_iterator row = rows.begin(); - row != rows.end(); ++row) { - dM.block((*row) + (*col) * M_rows, q_start, 1, q_subvector_size) = - dM_submatrix.row(index++); - } - } -} - -template -void setSubMatrixGradient( - // TODO(#2274) Fix NOLINTNEXTLINE(runtime/references). - Eigen::MatrixBase& dM, - const Eigen::MatrixBase& dM_submatrix, - const std::array& rows, const std::array& cols, - typename DerivedA::Index M_rows, typename DerivedA::Index q_start = 0, - typename DerivedA::Index q_subvector_size = QSubvectorSize) { - if (q_subvector_size == Eigen::Dynamic) { - q_subvector_size = dM.cols() - q_start; - } - int index = 0; - for (typename std::array::const_iterator col = cols.begin(); - col != cols.end(); ++col) { - for (typename std::array::const_iterator row = rows.begin(); - row != rows.end(); ++row) { - dM.template block<1, QSubvectorSize>((*row) + (*col) * M_rows, q_start, 1, - q_subvector_size) = - dM_submatrix.row(index++); - } - } -} - -template -void setSubMatrixGradient( - // TODO(#2274) Fix NOLINTNEXTLINE(runtime/references). - Eigen::MatrixBase& dM, - const Eigen::MatrixBase& dM_submatrix, int row, int col, - typename DerivedDM::Index M_rows, typename DerivedDM::Index q_start = 0, - typename DerivedDM::Index q_subvector_size = QSubvectorSize) { - if (q_subvector_size == Eigen::Dynamic) { - q_subvector_size = dM.cols() - q_start; - } - dM.template block<1, QSubvectorSize>(row + col * M_rows, q_start, 1, - q_subvector_size) = dM_submatrix; -} - -} // namespace math -} // namespace maliput::drake diff --git a/include/maliput/drake/math/gray_code.h b/include/maliput/drake/math/gray_code.h deleted file mode 100644 index 2bbc006..0000000 --- a/include/maliput/drake/math/gray_code.h +++ /dev/null @@ -1,61 +0,0 @@ -#pragma once - -#include - -#include "maliput/drake/common/drake_assert.h" - -namespace maliput::drake { -namespace math { -/** - * GrayCodesMatrix::type returns an Eigen matrix of integers. The size of this - * matrix is determined by the number of digits in the Gray code. - */ -template -struct GrayCodesMatrix { - static_assert(NumDigits >= 0 && NumDigits <= 30, "NumDigits out of range."); - typedef typename Eigen::Matrix - type; -}; - -template<> -struct GrayCodesMatrix { - typedef typename Eigen::MatrixXi type; -}; - -/** - * Returns a matrix whose i'th row is the Gray code for integer i. - * @tparam NumDigits The number of digits in the Gray code. - * @param num_digits The number of digits in the Gray code. - * @return M. M is a matrix of size 2ᵏ x k, where `k` is `num_digits`. - * M.row(i) is the Gray code for integer i. - */ -template -typename GrayCodesMatrix::type -CalculateReflectedGrayCodes(int num_digits = NumDigits) { - MALIPUT_DRAKE_DEMAND(num_digits >= 0); - int num_codes = num_digits <= 0 ? 0 : 1 << num_digits; - typename GrayCodesMatrix::type gray_codes(num_codes, num_digits); - // TODO(hongkai.dai): implement this part more efficiently. - for (int i = 0; i < num_codes; ++i) { - int gray_code = i ^ (i >> 1); - for (int j = 0; j < num_digits; ++j) { - gray_codes(i, j) = - (gray_code & (1 << (num_digits - j - 1))) >> (num_digits - j - 1); - } - } - return gray_codes; -} - -/** - * Converts the Gray code to an integer. For example - * (0, 0) -> 0 - * (0, 1) -> 1 - * (1, 1) -> 2 - * (1, 0) -> 3 - * @param gray_code The N-digit Gray code, where N is gray_code.rows() - * @return The integer represented by the Gray code `gray_code`. - */ -int GrayCodeToInteger(const Eigen::Ref& gray_code); -} // namespace math -} // namespace maliput::drake diff --git a/include/maliput/drake/math/hopf_coordinate.h b/include/maliput/drake/math/hopf_coordinate.h deleted file mode 100644 index bd17093..0000000 --- a/include/maliput/drake/math/hopf_coordinate.h +++ /dev/null @@ -1,79 +0,0 @@ -/// @file -/// Hopf coodinates parameterizes SO(3) locally as the Cartesian product of a -/// one-sphere and a two-sphere S¹ x S². Computationally, each rotation in the -/// Hopf coordinates can be written as (θ, φ, ψ), in which ψ parameterizes the -/// circle S¹ and has a range of 2π, and θ, φ represent the spherical -/// coordinates for S², with the range of π and 2π respectively. - -#pragma once - -#include - -#include -#include - -#include "maliput/drake/common/drake_assert.h" -#include "maliput/drake/common/eigen_types.h" - -namespace maliput::drake { -namespace math { -/** - * Transforms Hopf coordinates to a quaternion w, x, y, z as - * w = cos(θ/2)cos(ψ/2) - * x = cos(θ/2)sin(ψ/2) - * y = sin(θ/2)cos(φ+ψ/2) - * z = sin(θ/2)sin(φ+ψ/2) - * The user can refer to equation 5 of - * Generating Uniform Incremental Grids on SO(3) Using the Hopf Fibration - * by Anna Yershova, Steven LaValle and Julie Mitchell, 2008 - * @param theta The θ angle. - * @param phi The φ angle. - * @param psi The ψ angle. - */ -template -const Eigen::Quaternion HopfCoordinateToQuaternion(const T& theta, - const T& phi, - const T& psi) { - using std::cos; - using std::sin; - const T cos_half_theta = cos(theta / 2); - const T sin_half_theta = sin(theta / 2); - const T phi_plus_half_psi = phi + psi / 2; - return Eigen::Quaternion(cos_half_theta * cos(psi / 2), - cos_half_theta * sin(psi / 2), - sin_half_theta * cos(phi_plus_half_psi), - sin_half_theta * sin(phi_plus_half_psi)); -} - -/** - * Convert a unit-length quaternion (w, x, y, z) to Hopf coordinate as - * if w >= 0 - * ψ = 2*atan2(x, w) - * else - * ψ = 2*atan2(-x, -w) - * φ = mod(atan2(z, y) - ψ/2, 2pi) - * θ = 2*atan2(√(y²+z²), √(w²+x²)) - * ψ is in the range of [-pi, pi]. - * φ is in the range of [0, 2pi]. - * θ is in the range of [0, pi]. - * @param quaternion A unit length quaternion. - * @return hopf_coordinate (θ, φ, ψ) as an Eigen vector. - */ -template -Vector3 QuaternionToHopfCoordinate(const Eigen::Quaternion& quaternion) { - using std::atan2; - const T psi = quaternion.w() >= T(0) - ? 2 * atan2(quaternion.x(), quaternion.w()) - : 2 * atan2(-quaternion.x(), -quaternion.w()); - const T phi_unwrapped = atan2(quaternion.z(), quaternion.y()) - psi / 2; - // The range of phi_unwrapped is [-1.5pi, 1.5pi] - const T phi = phi_unwrapped >= 0 ? phi_unwrapped : phi_unwrapped + 2 * M_PI; - using std::pow; - using std::sqrt; - const T theta = - 2 * atan2(sqrt(pow(quaternion.y(), 2) + pow(quaternion.z(), 2)), - sqrt(pow(quaternion.w(), 2) + pow(quaternion.x(), 2))); - return Vector3(theta, phi, psi); -} -} // namespace math. -} // namespace maliput::drake diff --git a/include/maliput/drake/math/jacobian.h b/include/maliput/drake/math/jacobian.h deleted file mode 100644 index 392a820..0000000 --- a/include/maliput/drake/math/jacobian.h +++ /dev/null @@ -1,171 +0,0 @@ -#pragma once - -#include -#include - -#include - -#include "maliput/drake/common/autodiff.h" - -namespace maliput::drake { -namespace math { - -/** Computes a matrix of AutoDiffScalars from which both the value and - the Jacobian of a function - @f[ - f:\mathbb{R}^{n\times m}\rightarrow\mathbb{R}^{p\times q} - @f] - (f: R^n*m -> R^p*q) can be extracted. - - The derivative vector for each AutoDiffScalar in the output contains the - derivatives with respect to all components of the argument @f$ x @f$. - - The return type of this function is a matrix with the `best' possible - AutoDiffScalar scalar type, in the following sense: - - If the number of derivatives can be determined at compile time, the - AutoDiffScalar derivative vector will have that fixed size. - - If the maximum number of derivatives can be determined at compile time, the - AutoDiffScalar derivative vector will have that maximum fixed size. - - If neither the number, nor the maximum number of derivatives can be - determined at compile time, the output AutoDiffScalar derivative vector - will be dynamically sized. - - @p f should have a templated call operator that maps an Eigen matrix - argument to another Eigen matrix. The scalar type of the output of @f$ f @f$ - need not match the scalar type of the input (useful in recursive calls to the - function to determine higher order derivatives). The easiest way to create an - @p f is using a C++14 generic lambda. - - The algorithm computes the Jacobian in chunks of up to @p MaxChunkSize - derivatives at a time. This has three purposes: - - It makes it so that derivative vectors can be allocated on the stack, - eliminating dynamic allocations and improving performance if the maximum - number of derivatives cannot be determined at compile time. - - It gives control over, and limits the number of required - instantiations of the call operator of f and all the functions it calls. - - Excessively large derivative vectors can result in CPU capacity cache - misses; even if the number of derivatives is fixed at compile time, it may - be better to break up into chunks if that means that capacity cache misses - can be prevented. - - @param f function - @param x function argument value at which Jacobian will be evaluated - @return AutoDiffScalar matrix corresponding to the Jacobian of f evaluated - at x. - */ -template -decltype(auto) jacobian(F &&f, Arg &&x) { - using Eigen::AutoDiffScalar; - using Eigen::Index; - using Eigen::Matrix; - - using ArgNoRef = typename std::remove_reference_t; - - // Argument scalar type. - using ArgScalar = typename ArgNoRef::Scalar; - - // Argument scalar type corresponding to return value of this function. - using ReturnArgDerType = Matrix; - using ReturnArgAutoDiffScalar = AutoDiffScalar; - - // Return type of this function. - using ReturnArgAutoDiffType = - decltype(x.template cast().eval()); - using ReturnType = decltype(f(std::declval())); - - // Scalar type of chunk arguments. - using ChunkArgDerType = - Matrix; - using ChunkArgAutoDiffScalar = AutoDiffScalar; - - // Allocate output. - ReturnType ret; - - // Compute derivatives chunk by chunk. - constexpr Index kMaxChunkSize = MaxChunkSize; - Index num_derivs = x.size(); - bool values_initialized = false; - for (Index deriv_num_start = 0; deriv_num_start < num_derivs; - deriv_num_start += kMaxChunkSize) { - // Compute chunk size. - Index num_derivs_to_go = num_derivs - deriv_num_start; - Index chunk_size = std::min(kMaxChunkSize, num_derivs_to_go); - - // Initialize chunk argument. - auto chunk_arg = x.template cast().eval(); - for (Index i = 0; i < x.size(); i++) { - chunk_arg(i).derivatives().setZero(chunk_size); - } - for (Index i = 0; i < chunk_size; i++) { - Index deriv_num = deriv_num_start + i; - chunk_arg(deriv_num).derivatives()(i) = ArgScalar(1); - } - - // Compute Jacobian chunk. - auto chunk_result = f(chunk_arg); - - // On first chunk, resize output to match chunk and copy values from chunk - // to result. - if (!values_initialized) { - ret.resize(chunk_result.rows(), chunk_result.cols()); - - for (Index i = 0; i < chunk_result.size(); i++) { - ret(i).value() = chunk_result(i).value(); - ret(i).derivatives().resize(num_derivs); - } - values_initialized = true; - } - - // Copy derivatives from chunk to result. - for (Index i = 0; i < chunk_result.size(); i++) { - // Intuitive thing to do, but results in problems with non-matching scalar - // types for recursive jacobian calls: - // ret(i).derivatives().segment(deriv_num_start, chunk_size) = - // chunk_result(i).derivatives(); - - // Instead, assign each element individually, making use of conversion - // constructors. - for (Index j = 0; j < chunk_size; j++) { - ret(i).derivatives()(deriv_num_start + j) = - chunk_result(i).derivatives()(j); - } - } - } - - return ret; -} - -/** Computes a matrix of AutoDiffScalars from which the value, Jacobian, - and Hessian of a function - @f[ - f:\mathbb{R}^{n\times m}\rightarrow\mathbb{R}^{p\times q} - @f] - (f: R^n*m -> R^p*q) can be extracted. - - The output is a matrix of nested AutoDiffScalars, being the result of calling - ::jacobian on a function that returns the output of ::jacobian, - called on @p f. - - @p MaxChunkSizeOuter and @p MaxChunkSizeInner can be used to control chunk - sizes (see ::jacobian). - - See ::jacobian for requirements on the function @p f and the argument - @p x. - - @param f function - @param x function argument value at which Hessian will be evaluated - @return AutoDiffScalar matrix corresponding to the Hessian of f evaluated at - x - */ -template -decltype(auto) hessian(F &&f, Arg &&x) { - auto jac_fun = [&](const auto &x_inner) { - return jacobian(f, x_inner); - }; - return jacobian(jac_fun, x); -} - -} // namespace math -} // namespace maliput::drake diff --git a/include/maliput/drake/math/knot_vector_type.h b/include/maliput/drake/math/knot_vector_type.h deleted file mode 100644 index 1c56341..0000000 --- a/include/maliput/drake/math/knot_vector_type.h +++ /dev/null @@ -1,17 +0,0 @@ -#pragma once - -namespace maliput::drake { -namespace math { - -/** -Enum representing types of knot vectors. "Uniform" refers to the spacing -between the knots. "Clamped" indicates that the first and last knots have -multiplicity equal to the order of the spline. - -Reference: -http://web.mit.edu/hyperbook/Patrikalakis-Maekawa-Cho/node17.html -*/ -enum class KnotVectorType { kUniform, kClampedUniform }; - -} // namespace math -} // namespace maliput::drake diff --git a/include/maliput/drake/math/matrix_util.h b/include/maliput/drake/math/matrix_util.h deleted file mode 100644 index 8a9104d..0000000 --- a/include/maliput/drake/math/matrix_util.h +++ /dev/null @@ -1,155 +0,0 @@ -#pragma once - -#include -#include -#include - -#include - -#include "maliput/drake/common/drake_assert.h" -#include "maliput/drake/common/drake_throw.h" -#include "maliput/drake/common/eigen_types.h" - -namespace maliput::drake { -namespace math { -/// Determines if a matrix is symmetric. If std::equal_to<>()(matrix(i, j), -/// matrix(j, i)) is true for all i, j, then the matrix is symmetric. -template -bool IsSymmetric(const Eigen::MatrixBase& matrix) { - using DerivedScalar = typename Derived::Scalar; - if (matrix.rows() != matrix.cols()) { - return false; - } - for (int i = 0; i < static_cast(matrix.rows()); ++i) { - for (int j = i + 1; j < static_cast(matrix.cols()); ++j) { - if (!std::equal_to()(matrix(i, j), matrix(j, i))) { - return false; - } - } - } - return true; -} - -/// Determines if a matrix is symmetric based on whether the difference between -/// matrix(i, j) and matrix(j, i) is smaller than @p precision for all i, j. -/// The precision is absolute. -/// Matrix with nan or inf entries is not allowed. -template -bool IsSymmetric(const Eigen::MatrixBase& matrix, - const typename Derived::Scalar& precision) { - if (!std::isfinite(precision)) { - throw std::runtime_error("Cannot accept nans or inf is IsSymmetric"); - } - using DerivedScalar = typename Derived::Scalar; - if (matrix.rows() != matrix.cols()) { - return false; - } - for (int i = 0; i < static_cast(matrix.rows()); ++i) { - if (!std::isfinite(matrix(i, i))) { - throw std::runtime_error("Cannot accept nans or inf is IsSymmetric"); - } - for (int j = i + 1; j < static_cast(matrix.rows()); ++j) { - if (!std::isfinite(matrix(i, j)) || !std::isfinite(matrix(j, i))) { - throw std::runtime_error("Cannot accept nans or inf is IsSymmetric"); - } - DerivedScalar diff = matrix(i, j) - matrix(j, i); - if (!Eigen::NumTraits::IsSigned) { - if (diff > precision) { - return false; - } - } else if (diff > precision || -diff > precision) { - return false; - } - } - } - return true; -} - -namespace internal { -template -void to_symmetric_matrix_from_lower_triangular_columns_impl( - int rows, const Eigen::MatrixBase& lower_triangular_columns, - Eigen::MatrixBase* symmetric_matrix) { - int count = 0; - for (int j = 0; j < rows; ++j) { - (*symmetric_matrix)(j, j) = lower_triangular_columns(count); - ++count; - for (int i = j + 1; i < rows; ++i) { - (*symmetric_matrix)(i, j) = lower_triangular_columns(count); - (*symmetric_matrix)(j, i) = lower_triangular_columns(count); - ++count; - } - } -} -} // namespace internal - -/// Given a column vector containing the stacked columns of the lower triangular -/// part of a square matrix, returning a symmetric matrix whose lower -/// triangular part is the same as the original matrix. -template -drake::MatrixX -ToSymmetricMatrixFromLowerTriangularColumns( - const Eigen::MatrixBase& lower_triangular_columns) { - int rows = (-1 + sqrt(1 + 8 * lower_triangular_columns.rows())) / 2; - - MALIPUT_DRAKE_ASSERT(rows * (rows + 1) / 2 == lower_triangular_columns.rows()); - MALIPUT_DRAKE_ASSERT(lower_triangular_columns.cols() == 1); - - maliput::drake::MatrixX symmetric_matrix(rows, rows); - - internal::to_symmetric_matrix_from_lower_triangular_columns_impl( - rows, lower_triangular_columns, &symmetric_matrix); - return symmetric_matrix; -} - -/// Given a column vector containing the stacked columns of the lower triangular -/// part of a square matrix, returning a symmetric matrix whose lower -/// triangular part is the same as the original matrix. -/// @tparam rows The number of rows in the symmetric matrix. -template -Eigen::Matrix -ToSymmetricMatrixFromLowerTriangularColumns( - const Eigen::MatrixBase& lower_triangular_columns) { - EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(Derived, rows * (rows + 1) / 2); - - Eigen::Matrix symmetric_matrix(rows, - rows); - - internal::to_symmetric_matrix_from_lower_triangular_columns_impl( - rows, lower_triangular_columns, &symmetric_matrix); - return symmetric_matrix; -} - -/// Checks if a matrix is symmetric (with tolerance @p symmetry_tolerance -- -/// @see IsSymmetric) and has all eigenvalues greater than @p -/// eigenvalue_tolerance. @p eigenvalue_tolerance must be >= 0 -- where 0 -/// implies positive semi-definite (but is of course subject to all of the -/// pitfalls of floating point). -/// -/// To consider the numerical robustness of the eigenvalue estimation, we -/// specifically check that min_eigenvalue >= eigenvalue_tolerance * max(1, -/// max_abs_eigenvalue). -template -bool IsPositiveDefinite(const Eigen::MatrixBase& matrix, - double eigenvalue_tolerance = 0.0, - double symmetry_tolerance = 0.0) { - MALIPUT_DRAKE_DEMAND(eigenvalue_tolerance >= 0); - MALIPUT_DRAKE_DEMAND(symmetry_tolerance >= 0); - if (!IsSymmetric(matrix, symmetry_tolerance)) return false; - - // Note: Eigen's documentation clearly warns against using the faster LDLT - // for this purpose, as the algorithm cannot handle indefinite matrices. - Eigen::SelfAdjointEigenSolver eigensolver( - matrix); - MALIPUT_DRAKE_THROW_UNLESS(eigensolver.info() == Eigen::Success); - // According to the Lapack manual, the absolute accuracy of eigenvalues is - // eps*max(|eigenvalues|), so I will write my tolerances relative to that. - // Anderson et al., Lapack User's Guide, 3rd ed. section 4.7, 1999. - const double max_abs_eigenvalue = - eigensolver.eigenvalues().cwiseAbs().maxCoeff(); - return eigensolver.eigenvalues().minCoeff() >= - eigenvalue_tolerance * std::max(1., max_abs_eigenvalue); -} - -} // namespace math -} // namespace maliput::drake diff --git a/include/maliput/drake/math/normalize_vector.h b/include/maliput/drake/math/normalize_vector.h deleted file mode 100644 index 4091ad4..0000000 --- a/include/maliput/drake/math/normalize_vector.h +++ /dev/null @@ -1,60 +0,0 @@ -#pragma once - -#include - -#include "maliput/drake/math/gradient.h" -#include "maliput/drake/math/gradient_util.h" - -namespace maliput::drake { -namespace math { -/** Computes the normalized vector, optionally with its gradient and second -derivative. -@param[in] x An N x 1 vector to be normalized. Must not be zero. -@param[out] x_norm The normalized vector (N x 1). -@param[out] dx_norm - If non-null, returned as an N x N matrix, - where dx_norm(i,j) = D x_norm(i)/D x(j). -@param[out] ddx_norm - If non-null, and dx_norm is non-null, returned as an N^2 x N matrix, - where ddx_norm.col(j) = D dx_norm/D x(j), with dx_norm stacked - columnwise. - -(D x / D y above means partial derivative of x with respect to y.) */ -template -void NormalizeVector( - const Eigen::MatrixBase& x, - // TODO(#2274) Fix NOLINTNEXTLINE(runtime/references). - typename Derived::PlainObject& x_norm, - typename maliput::drake::math::Gradient::type* dx_norm = nullptr, - typename maliput::drake::math::Gradient::type* ddx_norm = nullptr) { - typename Derived::Scalar xdotx = x.squaredNorm(); - typename Derived::Scalar norm_x = sqrt(xdotx); - x_norm = x / norm_x; - - if (dx_norm) { - dx_norm->setIdentity(x.rows(), x.rows()); - (*dx_norm) -= x * x.transpose() / xdotx; - (*dx_norm) /= norm_x; - - if (ddx_norm) { - auto dx_norm_transpose = transposeGrad(*dx_norm, x.rows()); - auto minus_ddx_norm_times_norm = matGradMultMat( - x_norm, x_norm.transpose(), (*dx_norm), dx_norm_transpose); - auto dnorm_inv = -x.transpose() / (xdotx * norm_x); - (*ddx_norm) = -minus_ddx_norm_times_norm / norm_x; - auto temp = (*dx_norm) * norm_x; - typename Derived::Index n = x.rows(); - for (int col = 0; col < n; col++) { - auto column_as_matrix = (dnorm_inv(0, col) * temp); - for (int row_block = 0; row_block < n; row_block++) { - ddx_norm->block(row_block * n, col, n, 1) += - column_as_matrix.col(row_block); - } - } - } - } -} -} // namespace math -} // namespace maliput::drake diff --git a/include/maliput/drake/math/orthonormal_basis.h b/include/maliput/drake/math/orthonormal_basis.h deleted file mode 100644 index a855c31..0000000 --- a/include/maliput/drake/math/orthonormal_basis.h +++ /dev/null @@ -1,61 +0,0 @@ -#pragma once - -#include - -#include "maliput/drake/common/drake_deprecated.h" -#include "maliput/drake/common/eigen_types.h" - -namespace maliput::drake { -namespace math { - -/// Creates a right-handed local basis from a given axis. Defines two other -/// arbitrary axes such that the basis is orthonormal. The basis is R_WL, where -/// W is the frame in which the input axis is expressed and L is a local basis -/// such that v_W = R_WL * v_L. -/// -/// @param[in] axis_index The index of the axis (in the range [0,2]), with -/// 0 corresponding to the x-axis, 1 corresponding to the -/// y-axis, and z-corresponding to the z-axis. -/// @param[in] axis_W The vector defining the basis's given axis expressed -/// in frame W. The vector need not be a unit vector: this -/// routine will normalize it. -/// @retval R_WL The computed basis. -/// @throws std::exception if the norm of @p axis_W is within 1e-10 to zero or -/// @p axis_index does not lie in the range [0,2]. -template -DRAKE_DEPRECATED("2021-10-01", "Use RotationMatrix::MakeFromOneVector().") -Matrix3 ComputeBasisFromAxis(int axis_index, const Vector3& axis_W) { - // Verify that the correct axis is given. - if (axis_index < 0 || axis_index > 2) - throw std::logic_error("Invalid axis specified: must be 0, 1, or 2."); - - // Verify that the vector is not nearly zero. - const double zero_tol = 1e-10; - const T norm = axis_W.norm(); - if (norm < zero_tol) - throw std::logic_error("Vector appears to be nearly zero."); - - // The axis corresponding to the smallest component of axis_W will be *most* - // perpendicular. - const Vector3 u(axis_W.cwiseAbs()); - int minAxis; - u.minCoeff(&minAxis); - Vector3 perpAxis = Vector3::Zero(); - perpAxis[minAxis] = 1; - const Vector3 axis_W_unit = axis_W / norm; - - // Now define additional vectors in the basis. - Vector3 v1_W = axis_W_unit.cross(perpAxis).normalized(); - Vector3 v2_W = axis_W_unit.cross(v1_W); - - // Set the columns of the matrix. - Matrix3 R_WL; - R_WL.col(axis_index) = axis_W_unit; - R_WL.col((axis_index + 1) % 3) = v1_W; - R_WL.col((axis_index + 2) % 3) = v2_W; - - return R_WL; -} - -} // namespace math -} // namespace maliput::drake diff --git a/include/maliput/drake/math/quadratic_form.h b/include/maliput/drake/math/quadratic_form.h deleted file mode 100644 index 02b7718..0000000 --- a/include/maliput/drake/math/quadratic_form.h +++ /dev/null @@ -1,144 +0,0 @@ -#pragma once - -#include - -#include - -namespace maliput::drake { -namespace math { -/** - * For a symmetric positive semidefinite matrix Y, decompose it into XᵀX, where - * the number of rows in X equals to the rank of Y. - * Notice that this decomposition is not unique. For any orthonormal matrix U, - * s.t UᵀU = identity, X_prime = UX also satisfies X_primeᵀX_prime = Y. Here - * we only return one valid decomposition. - * @param Y A symmetric positive semidefinite matrix. - * @param zero_tol We will need to check if some value (for example, the - * absolute value of Y's eigenvalues) is smaller than zero_tol. If it is, then - * we deem that value as 0. - * @retval X. The matrix X satisfies XᵀX = Y and X.rows() = rank(Y). - * @pre 1. Y is positive semidefinite. - * 2. zero_tol is non-negative. - * @throws std::exception when the pre-conditions are not satisfied. - * @note We only use the lower triangular part of Y. - */ -Eigen::MatrixXd DecomposePSDmatrixIntoXtransposeTimesX( - const Eigen::Ref& Y, double zero_tol); - -/** - * Rewrite a quadratic form xᵀQx + bᵀx + c to - * (Rx+d)ᵀ(Rx+d) - * where - * 
- * RᵀR = Q
- * Rᵀd = b / 2
- * dᵀd = c
- *
- * - * This decomposition requires the matrix - * 
- * ⌈Q     b/2⌉
- * ⌊bᵀ/2    c⌋
- *
- * to be positive semidefinite. - * - * We return R and d with the minimal number of rows, namely the rows of R - * and d equal to the rank of the matrix - * 
- * ⌈Q     b/2⌉
- * ⌊bᵀ/2    c⌋
- *
- * - * Notice that R might have more rows than Q, For example, the quadratic - * expression x² + 2x + 5 =(x+1)² + 2², it can be decomposed as - * 
- * ⎛⌈1⌉ * x + ⌈1⌉⎞ᵀ * ⎛⌈1⌉ * x + ⌈1⌉⎞
- * ⎝⌊0⌋       ⌊2⌋⎠    ⎝⌊0⌋       ⌊2⌋⎠
- *
- * Here R has 2 rows while Q only has 1 row. - * - * On the other hand the quadratic expression x² + 2x + 1 can be decomposed - * as (x+1) * (x+1), where R has 1 row, same as Q. - * - * Also notice that this decomposition is not unique. For example, with any - * permutation matrix P, we can define - * 
- * R₁ = P*R
- * d₁ = P*d
- *
- * Then (R₁*x+d₁)ᵀ(R₁*x+d₁) gives the same quadratic form. - * @param Q The square matrix. - * @param b The vector containing the linear coefficients. - * @param c The constant term. - * @param tol We will determine if this quadratic form is always non-negative, - * by checking the Eigen values of the matrix - * [Q b/2] - * [bᵀ/2 c] - * are all greater than -tol. @default is 0. - * @retval (R, d). R and d have the same number of rows. R.cols() == x.rows(). - * R.rows() equals to the rank of the matrix - * 
- *    [Q    b/2]
- *    [bᵀ/2   c]
- *
- * @pre 1. The quadratic form is always non-negative, namely the matrix - * 
- *         [Q    b/2]
- *         [bᵀ/2   c]
- *
- * is positive semidefinite. - * 2. `Q` and `b` are of the correct size. - * 3. `tol` is non-negative. - * @throws std::exception if the precondition is not satisfied. - */ -std::pair -DecomposePositiveQuadraticForm(const Eigen::Ref& Q, - const Eigen::Ref& b, - double c, double tol = 0); - -/** Given two quadratic forms, x'Sx > 0 and x'Px, (with P symmetric and full - * rank), finds a change of variables x = Ty, which simultaneously diagonalizes - * both forms (as inspired by "balanced truncation" in model-order reduction - * [1]). In this note, we use abs(M) to indicate the elementwise absolute - * value. - * - * Adapting from [1], we observe that there is a family of coordinate systems - * that can simultaneously diagonalize T'ST and T'PT. Using D to denote a - * diagonal matrix, we call the result S-normal if T'ST = I and abs(T'PT) = D⁻², - * call it P-normal if T'ST = D² and abs(T'PT) = I, and call it "balanced" if - * T'ST = D and abs(T'PT) = D⁻¹. Note that if P > 0, then T'PT = D⁻¹. - * - * We find x=Ty such that T'ST = D and abs(T'PT) = D⁻¹, where D is diagonal. The - * recipe is: - * - Factorize S = LL', and choose R=L⁻¹. - * - Take svd(RPR') = UΣV', and note that U=V for positive definite matrices, - * and V is U up to a sign flip of the singular vectors for all symmetric - * matrices. - * - Choose T = R'U Σ^{-1/4}, where the matrix exponent can be taken elementwise - * because Σ is diagonal. This gives T'ST = Σ^{-1/2} (by using U'U=I), and - * abs(T'PT) = Σ^{1/2}. If P > 0, then T'PT = Σ^{1/2}. - * - * Note that the numerical "balancing" can address the absolute scaling of the - * quadratic forms, but not the relative scaling. To understand this, consider - * the scalar case: we have two quadratic functions, sx² and px², with s>0, p>0. - * We'd like to choose x=Ty so that sT²y² and pT²y² are "balanced" (we'd like - * them both to be close to y²). We'll choose T=p^{-1/4}s^{-1/4}, which gives - * sx² = sqrt(s/p)y², and px² = sqrt(p/s)y². For instance if s=1e8 and p=1e8, - * then t=1e-4 and st^2 = pt^2 = 1. But if s=10, p=1e7, then t=0.01, and st^2 = - * 1e-3, pt^2 = 1e3. - * - * In the matrix case, the absolute scaling is important -- it ensures that the - * two quadratic forms have the same matrix condition number and makes them as - * close as possible to 1. Besides absolute scaling, in the matrix case the - * balancing transform diagonalizes both quadratic forms. - * - * [1] B. Moore, “Principal component analysis in linear systems: - * Controllability, observability, and model reduction,” IEEE Trans. Automat. - * Contr., vol. 26, no. 1, pp. 17–32, Feb. 1981. - */ -Eigen::MatrixXd BalanceQuadraticForms( - const Eigen::Ref& S, - const Eigen::Ref& P); - -} // namespace math -} // namespace maliput::drake diff --git a/include/maliput/drake/math/quaternion.h b/include/maliput/drake/math/quaternion.h deleted file mode 100644 index 39b2c95..0000000 --- a/include/maliput/drake/math/quaternion.h +++ /dev/null @@ -1,394 +0,0 @@ -/// @file -/// Utilities for arithmetic on quaternions. -// @internal Eigen's 4-argument Quaternion constructor uses (w, x, y, z) order. -// HOWEVER: If you use Eigen's 1-argument Quaternion constructor, where the one -// argument is a 4-element Vector, the elements must be in (x, y, z, w) order! -// So, the following two calls will give you the SAME quaternion: -// Quaternion(q(0), q(1), q(2), q(3)); -// Quaternion(Vector4d(q(3), q(0), q(1), q(2))) -// which is gross and will cause you much pain. See: -// http://eigen.tuxfamily.org/dox/classEigen_1_1Quaternion.html#a91b6ea2cac13ab2d33b6e74818ee1490 - -#pragma once - -#include - -#include - -#include "maliput/drake/common/drake_assert.h" -#include "maliput/drake/common/drake_bool.h" -#include "maliput/drake/common/drake_deprecated.h" -#include "maliput/drake/common/eigen_types.h" -#include "maliput/drake/common/is_approx_equal_abstol.h" -#include "maliput/drake/math/rotation_matrix.h" - -namespace maliput::drake { -namespace math { - -/** - * Returns a unit quaternion that represents the same orientation as `q1`, - * and has the "shortest" geodesic distance on the unit sphere to `q0`. - */ -template Eigen::Quaternion ClosestQuaternion( - const Eigen::Quaternion& q0, - const Eigen::Quaternion& q1) { - Eigen::Quaternion q = q1; - if (q0.dot(q) < 0) - q.coeffs() *= -1; - q.normalize(); - return q; -} - -template -Vector4 quatConjugate( - const Eigen::MatrixBase& q) { - // TODO(hongkai.dai@tri.global): Switch to Eigen's Quaternion when we fix - // the range problem in Eigen - static_assert(Derived::SizeAtCompileTime == 4, "Wrong size."); - Vector4 q_conj; - q_conj << q(0), -q(1), -q(2), -q(3); - return q_conj; -} - -template -Vector4 quatProduct( - const Eigen::MatrixBase& q1, - const Eigen::MatrixBase& q2) { - // TODO(hongkai.dai@tri.global): Switch to Eigen's Quaternion when we fix - // the range problem in Eigen - static_assert(Derived1::SizeAtCompileTime == 4, "Wrong size."); - static_assert(Derived2::SizeAtCompileTime == 4, "Wrong size."); - - Eigen::Quaternion q1_eigen(q1(0), q1(1), q1(2), - q1(3)); - Eigen::Quaternion q2_eigen(q2(0), q2(1), q2(2), - q2(3)); - auto ret_eigen = q1_eigen * q2_eigen; - Vector4 r; - r << ret_eigen.w(), ret_eigen.x(), ret_eigen.y(), ret_eigen.z(); - - return r; -} - -template -Vector3 quatRotateVec( - const Eigen::MatrixBase& q, - const Eigen::MatrixBase& v) { - // TODO(hongkai.dai@tri.global): Switch to Eigen's Quaternion when we fix - // the range problem in Eigen - static_assert(DerivedQ::SizeAtCompileTime == 4, "Wrong size."); - static_assert(DerivedV::SizeAtCompileTime == 3, "Wrong size."); - - Vector4 v_quat; - v_quat << 0, v; - auto q_times_v = quatProduct(q, v_quat); - auto q_conj = quatConjugate(q); - auto v_rot = quatProduct(q_times_v, q_conj); - Vector3 r = v_rot.template bottomRows<3>(); - return r; -} - -template -Vector4 quatDiff( - const Eigen::MatrixBase& q1, - const Eigen::MatrixBase& q2) { - // TODO(hongkai.dai@tri.global): Switch to Eigen's Quaternion when we fix - // the range problem in Eigen - return quatProduct(quatConjugate(q1), q2); -} - -template -typename Derived1::Scalar quatDiffAxisInvar( - const Eigen::MatrixBase& q1, - const Eigen::MatrixBase& q2, - const Eigen::MatrixBase& u) { - // TODO(hongkai.dai@tri.global): Switch to Eigen's Quaternion when we fix - // the range problem in Eigen - static_assert(DerivedU::SizeAtCompileTime == 3, "Wrong size."); - auto r = quatDiff(q1, q2); - return -2.0 + 2 * r(0) * r(0) + - 2 * pow(u(0) * r(1) + u(1) * r(2) + u(2) * r(3), 2); -} - -/** - * This function tests whether a quaternion is in "canonical form" meaning that - * it tests whether the quaternion [w, x, y, z] has a non-negative w value. - * Example: [-0.3, +0.4, +0.5, +0.707] is not in canonical form. - * Example: [+0.3, -0.4, -0.5, -0.707] is in canonical form. - * @param quat Quaternion [w, x, y, z] that relates two right-handed - * orthogonal unitary bases e.g., Ax, Ay, Az (A) to Bx, By, Bz (B). - * Note: quat is analogous to the rotation matrix R_AB. - * @return `true` if quat.w() is nonnegative (in canonical form), else `false`. - */ -template -bool is_quaternion_in_canonical_form(const Eigen::Quaternion& quat) { - return quat.w() >= 0.0; -} - - -/** - * This function returns a quaternion in its "canonical form" meaning that - * it returns a quaternion [w, x, y, z] with a non-negative w. - * For example, if passed a quaternion [-0.3, +0.4, +0.5, +0.707], the function - * returns the quaternion's canonical form [+0.3, -0.4, -0.5, -0.707]. - * @param quat Quaternion [w, x, y, z] that relates two right-handed - * orthogonal unitary bases e.g., Ax, Ay, Az (A) to Bx, By, Bz (B). - * Note: quat is analogous to the rotation matrix R_AB. - * @return Canonical form of quat, which means that either the original quat - * is returned or a quaternion representing the same orientation but with - * negated [w, x, y, z], to ensure a positive w in returned quaternion. - */ -template -Eigen::Quaternion QuaternionToCanonicalForm( - const Eigen::Quaternion& quat ) { - return is_quaternion_in_canonical_form(quat) ? quat : - Eigen::Quaternion(-quat.w(), -quat.x(), -quat.y(), -quat.z()); -} - - -/** - * This function tests whether two quaternions represent the same orientation. - * This function converts each quaternion to its canonical form and tests - * whether the absolute value of the difference in corresponding elements of - * these canonical quaternions is within tolerance. - * @param quat1 Quaternion [w, x, y, z] that relates two right-handed - * orthogonal unitary bases e.g., Ax, Ay, Az (A) to Bx, By, Bz (B). - * Note: quat is analogous to the rotation matrix R_AB. - * @param quat2 Quaternion with a description analogous to quat1. - * @param tolerance Nonnegative real scalar defining the allowable difference - * in the orientation described by quat1 and quat2. - * @return `true` if quat1 and quat2 represent the same orientation (to within - * tolerance), otherwise `false`. - */ -template -bool AreQuaternionsEqualForOrientation( - const Eigen::Quaternion& quat1, - const Eigen::Quaternion& quat2, - const T tolerance) { - const Eigen::Quaternion quat1_canonical = QuaternionToCanonicalForm(quat1); - const Eigen::Quaternion quat2_canonical = QuaternionToCanonicalForm(quat2); - return quat1_canonical.isApprox(quat2_canonical, tolerance); -} - -// Note: To avoid dependence on Eigen's internal ordering of elements in its -// Quaternion class, herein we use `e0 = quat.w()', `e1 = quat.x()`, etc. -// Return value `quatDt` *does* have a specific order as defined above. -/** This function calculates a quaternion's time-derivative from its quaternion - * and angular velocity. Algorithm from [Kane, 1983] Section 1.13, Pages 58-59. - * - * - [Kane, 1983] "Spacecraft Dynamics," McGraw-Hill Book Co., New York, 1983. - * (With P. W. Likins and D. A. Levinson). Available for free .pdf download: - * https://ecommons.cornell.edu/handle/1813/637 - * - * @param quat_AB Quaternion [w, x, y, z] that relates two right-handed - * orthogonal unitary bases e.g., Ax, Ay, Az (A) to Bx, By, Bz (B). - * Note: quat_AB is analogous to the rotation matrix R_AB. - * @param w_AB_B B's angular velocity in A, expressed in B. - * @retval quatDt Time-derivative of quat_AB, i.e., [ẇ, ẋ, ẏ, ż]. - */ -template -Vector4 CalculateQuaternionDtFromAngularVelocityExpressedInB( - const Eigen::Quaternion& quat_AB, const Vector3& w_AB_B ) { - const T e0 = quat_AB.w(), e1 = quat_AB.x(), - e2 = quat_AB.y(), e3 = quat_AB.z(); - const T wx = w_AB_B[0], wy = w_AB_B[1], wz = w_AB_B[2]; - - const T e0Dt = (-e1*wx - e2*wy - e3*wz) / 2; - const T e1Dt = (e0*wx - e3*wy + e2*wz) / 2; - const T e2Dt = (e3*wx + e0*wy - e1*wz) / 2; - const T e3Dt = (-e2*wx + e1*wy + e0*wz) / 2; - - return Vector4(e0Dt, e1Dt, e2Dt, e3Dt); -} - - -// Note: To avoid dependence on Eigen's internal ordering of elements in its -// Quaternion class, herein we use `e0 = quat.w()', `e1 = quat.x()`, etc. -// Parameter `quatDt` *does* have a specific order as defined above. -/** This function calculates angular velocity from a quaternion and its time- - * derivative. Algorithm from [Kane, 1983] Section 1.13, Pages 58-59. - * - * - [Kane, 1983] "Spacecraft Dynamics," McGraw-Hill Book Co., New York, 1983. - * (with P. W. Likins and D. A. Levinson). Available for free .pdf download: - * https://ecommons.cornell.edu/handle/1813/637 - * - * @param quat_AB Quaternion [w, x, y, z] that relates two right-handed - * orthogonal unitary bases e.g., Ax, Ay, Az (A) to Bx, By, Bz (B). - * Note: quat_AB is analogous to the rotation matrix R_AB. - * @param quatDt Time-derivative of `quat_AB`, i.e. [ẇ, ẋ, ẏ, ż]. - * @retval w_AB_B B's angular velocity in A, expressed in B. - */ -template -Vector3 CalculateAngularVelocityExpressedInBFromQuaternionDt( - const Eigen::Quaternion& quat_AB, const Vector4& quatDt) { - const T e0 = quat_AB.w(), e1 = quat_AB.x(), - e2 = quat_AB.y(), e3 = quat_AB.z(); - const T e0Dt = quatDt[0], e1Dt = quatDt[1], - e2Dt = quatDt[2], e3Dt = quatDt[3]; - - const T wx = 2*(-e1*e0Dt + e0*e1Dt + e3*e2Dt - e2*e3Dt); - const T wy = 2*(-e2*e0Dt - e3*e1Dt + e0*e2Dt + e1*e3Dt); - const T wz = 2*(-e3*e0Dt + e2*e1Dt - e1*e2Dt + e0*e3Dt); - - return Vector3(wx, wy, wz); -} - - -/** This function calculates how well a quaternion and its time-derivative - * satisfy the quaternion time-derivative constraint specified in [Kane, 1983] - * Section 1.13, equations 12-13, page 59. For a quaternion [w, x, y, z], - * the quaternion must satisfy: w^2 + x^2 + y^2 + z^2 = 1, hence its - * time-derivative must satisfy: 2*(w*ẇ + x*ẋ + y*ẏ + z*ż) = 0. - * - * - [Kane, 1983] "Spacecraft Dynamics," McGraw-Hill Book Co., New York, 1983. - * (with P. W. Likins and D. A. Levinson). Available for free .pdf download: - * https://ecommons.cornell.edu/handle/1813/637 - * - * @param quat Quaternion [w, x, y, z] that relates two right-handed - * orthogonal unitary bases e.g., Ax, Ay, Az (A) to Bx, By, Bz (B). - * Note: A quaternion like quat_AB is analogous to the rotation matrix R_AB. - * @param quatDt Time-derivative of `quat`, i.e., [ẇ, ẋ, ẏ, ż]. - * @retval quaternionDt_constraint_violation The amount the time- - * derivative of the quaternion constraint has been violated, which may be - * positive or negative (0 means the constraint is perfectly satisfied). - */ -template -T CalculateQuaternionDtConstraintViolation(const Eigen::Quaternion& quat, - const Vector4& quatDt) { - const T w = quat.w(), x = quat.x(), y = quat.y(), z = quat.z(); - const T wDt = quatDt[0], xDt = quatDt[1], yDt = quatDt[2], zDt = quatDt[3]; - const T quaternionDt_constraint_violation = 2*(w*wDt + x*xDt + y*yDt + z*zDt); - return quaternionDt_constraint_violation; -} - -/** This function tests if a quaternion satisfies the quaternion constraint - * specified in [Kane, 1983] Section 1.3, equation 4, page 12, i.e., a - * quaternion [w, x, y, z] must satisfy: w^2 + x^2 + y^2 + z^2 = 1. - * - * - [Kane, 1983] "Spacecraft Dynamics," McGraw-Hill Book Co., New York, 1983. - * (with P. W. Likins and D. A. Levinson). Available for free .pdf download: - * https://ecommons.cornell.edu/handle/1813/637 - * - * @param quat Quaternion [w, x, y, z] that relates two right-handed - * orthogonal unitary bases e.g., Ax, Ay, Az (A) to Bx, By, Bz (B). - * Note: A quaternion like quat_AB is analogous to the rotation matrix R_AB. - * @param tolerance Tolerance for quaternion constraint, i.e., how much is - * w^2 + x^2 + y^2 + z^2 allowed to differ from 1. - * @return `true` if the quaternion constraint is satisfied within tolerance. - */ -template -bool IsQuaternionValid(const Eigen::Quaternion& quat, - const double tolerance) { - using std::abs; - const T quat_norm_error = abs(1.0 - quat.norm()); - return (quat_norm_error <= tolerance); -} - - -/** This function tests if a quaternion satisfies the time-derivative constraint - * specified in [Kane, 1983] Section 1.13, equation 13, page 59. A quaternion - * [w, x, y, z] must satisfy w^2 + x^2 + y^2 + z^2 = 1, hence its - * time-derivative must satisfy 2*(w*ẇ + x*ẋ + y*ẏ + z*ż) = 0. - * Note: To accurately test whether the time-derivative quaternion constraint - * is satisfied, the quaternion constraint is also tested to be accurate. - * - * - [Kane, 1983] "Spacecraft Dynamics," McGraw-Hill Book Co., New York, 1983. - * (with P. W. Likins and D. A. Levinson). Available for free .pdf download: - * https://ecommons.cornell.edu/handle/1813/637 - * - * @param quat Quaternion [w, x, y, z] that relates two right-handed - * orthogonal unitary bases e.g., Ax, Ay, Az (A) to Bx, By, Bz (B). - * Note: A quaternion like quat_AB is analogous to the rotation matrix R_AB. - * @param quatDt Time-derivative of `quat`, i.e., [ẇ, ẋ, ẏ, ż]. - * @param tolerance Tolerance for quaternion constraints. - * @return `true` if both of the two previous constraints are within tolerance. - */ -template -bool IsBothQuaternionAndQuaternionDtOK(const Eigen::Quaternion& quat, - const Vector4& quatDt, - const double tolerance) { - using std::abs; - - // For an accurate test, the quaternion should be reasonably accurate. - if ( !IsQuaternionValid(quat, tolerance) ) return false; - - const T quatDt_test = CalculateQuaternionDtConstraintViolation(quat, quatDt); - return abs(quatDt_test) <= tolerance; -} - - -/** This function tests if a quaternion and a quaternions time-derivative - * can calculate and match an angular velocity to within a tolerance. - * Note: This function first tests if the quaternion [w, x, y, z] satisfies - * w^2 + x^2 + y^2 + z^2 = 1 (to within tolerance) and if its time-derivative - * satisfies w*ẇ + x*ẋ + y*ẏ + z*ż = 0 (to within tolerance). Lastly, it - * tests if each element of the angular velocity calculated from quat and quatDt - * is within tolerance of w_B (described below). - * @param quat Quaternion [w, x, y, z] that relates two right-handed - * orthogonal unitary bases e.g., Ax, Ay, Az (A) to Bx, By, Bz (B). - * Note: A quaternion like quat_AB is analogous to the rotation matrix R_AB. - * @param quatDt Time-derivative of `quat`, i.e., [ẇ, ẋ, ẏ, ż]. - * @param w_B Rigid body B's angular velocity in frame A, expressed in B. - * @param tolerance Tolerance for quaternion constraints. - * @return `true` if all three of the previous constraints are within tolerance. - */ -template -bool IsQuaternionAndQuaternionDtEqualAngularVelocityExpressedInB( - const Eigen::Quaternion& quat, - const Vector4& quatDt, - const Vector3& w_B, - const double tolerance) { - // Ensure time-derivative of quaternion satisfies quarternionDt test. - if ( !math::IsBothQuaternionAndQuaternionDtOK(quat, quatDt, tolerance) ) - return false; - - const Eigen::Vector3d w_from_quatDt = - math::CalculateAngularVelocityExpressedInBFromQuaternionDt(quat, quatDt); - return is_approx_equal_abstol(w_from_quatDt, w_B, tolerance); -} - -namespace internal { -/* - * Given a unit-length quaternion, convert this quaternion to angle-axis - * representation. Note that we always choose the angle to be within [0, pi]. - * This function is the same as Eigen::AngleAxis(Eigen::Quaternion z), - * but it works for T=symbolic::Expression. - * @note If you just want to use T=double, then call - * Eigen::AngleAxisd(Eigen::Quaterniond z). We add this function only to support - * templated function that allows T=symbolic::Expression. - * @param quaternion Must be a unit quaternion. - * @return angle_axis The angle-axis representation of the quaternion. Note - * that the angle is within [0, pi]. - */ -template -Eigen::AngleAxis QuaternionToAngleAxisLikeEigen( - const Eigen::Quaternion& quaternion) { - Eigen::AngleAxis result; - using std::atan2; - T sin_half_angle_abs = quaternion.vec().norm(); - if constexpr (scalar_predicate::is_bool) { - if (sin_half_angle_abs < Eigen::NumTraits::epsilon()) { - sin_half_angle_abs = quaternion.vec().stableNorm(); - } - } - using std::abs; - result.angle() = T(2.) * atan2(sin_half_angle_abs, abs(quaternion.w())); - const Vector3 unit_axis(T(1.), T(0.), T(0.)); - // We use if_then_else here (instead of if statement) because using "if" - // with symbolic expression causes runtime error in this case (The symbolic - // formula needs to evaluate with an empty symbolic Environment). - const boolean is_sin_angle_zero = sin_half_angle_abs == T(0.); - const boolean is_w_negative = quaternion.w() < T(0.); - const T axis_sign = if_then_else(is_w_negative, T(-1), T(1)); - for (int i = 0; i < 3; ++i) { - result.axis()(i) = - if_then_else(is_sin_angle_zero, unit_axis(i), - axis_sign * quaternion.vec()(i) / sin_half_angle_abs); - } - - return result; -} -} // namespace internal - -} // namespace math -} // namespace maliput::drake diff --git a/include/maliput/drake/math/random_rotation.h b/include/maliput/drake/math/random_rotation.h deleted file mode 100644 index 1b289ac..0000000 --- a/include/maliput/drake/math/random_rotation.h +++ /dev/null @@ -1,75 +0,0 @@ -#pragma once - -#include - -#include - -#include "maliput/drake/common/constants.h" -#include "maliput/drake/common/eigen_types.h" -#include "maliput/drake/common/random.h" -#include "maliput/drake/math/quaternion.h" -#include "maliput/drake/math/roll_pitch_yaw.h" - -namespace maliput::drake { -namespace math { - -/// Generates a rotation (in the quaternion representation) that rotates a -/// point on the unit sphere to another point on the unit sphere with a uniform -/// distribution over the sphere. -/// This method is briefly explained in -/// http://planning.cs.uiuc.edu/node198.html, a full explanation can be found in -/// K. Shoemake. Uniform Random Rotations in D. Kirk, editor, Graphics Gems III, -/// pages 124-132. Academic, New York, 1992. -template -Eigen::Quaternion UniformlyRandomQuaternion(Generator* generator) { - MALIPUT_DRAKE_DEMAND(generator != nullptr); - std::uniform_real_distribution uniform(0., 1.); - const T u1 = uniform(*generator); - const T u2 = uniform(*generator); - const T u3 = uniform(*generator); - using std::sqrt; - const T sqrt_one_minus_u1 = sqrt(1. - u1); - const T sqrt_u1 = sqrt(u1); - using std::cos; - using std::sin; - return Eigen::Quaternion(sqrt_one_minus_u1 * sin(2 * M_PI * u2), - sqrt_one_minus_u1 * cos(2 * M_PI * u2), - sqrt_u1 * sin(2 * M_PI * u3), - sqrt_u1 * cos(2 * M_PI * u3)); -} - -/// Generates a rotation (in the axis-angle representation) that rotates a -/// point on the unit sphere to another point on the unit sphere with a uniform -/// distribution over the sphere. -template -Eigen::AngleAxis UniformlyRandomAngleAxis(Generator* generator) { - MALIPUT_DRAKE_DEMAND(generator != nullptr); - const Eigen::Quaternion quaternion = - UniformlyRandomQuaternion(generator); - return internal::QuaternionToAngleAxisLikeEigen(quaternion); -} - -/// Generates a rotation (in the rotation matrix representation) that rotates a -/// point on the unit sphere to another point on the unit sphere with a uniform -/// distribution over the sphere. -template -RotationMatrix UniformlyRandomRotationMatrix(Generator* generator) { - MALIPUT_DRAKE_DEMAND(generator != nullptr); - const Eigen::Quaternion quaternion = - UniformlyRandomQuaternion(generator); - return RotationMatrix(quaternion); -} - -/// Generates a rotation (in the roll-pitch-yaw representation) that rotates a -/// point on the unit sphere to another point on the unit sphere with a uniform -/// distribution over the sphere. -template -Vector3 UniformlyRandomRPY(Generator* generator) { - MALIPUT_DRAKE_DEMAND(generator != nullptr); - const Eigen::Quaternion q = UniformlyRandomQuaternion(generator); - const RollPitchYaw rpy(q); - return rpy.vector(); -} - -} // namespace math -} // namespace maliput::drake diff --git a/include/maliput/drake/math/rigid_transform.h b/include/maliput/drake/math/rigid_transform.h deleted file mode 100644 index ef2e111..0000000 --- a/include/maliput/drake/math/rigid_transform.h +++ /dev/null @@ -1,670 +0,0 @@ -#pragma once - -#include - -#include - -#include "maliput/drake/common/default_scalars.h" -#include "maliput/drake/common/drake_assert.h" -#include "maliput/drake/common/drake_bool.h" -#include "maliput/drake/common/drake_copyable.h" -#include "maliput/drake/common/eigen_types.h" -#include "maliput/drake/common/never_destroyed.h" -#include "maliput/drake/math/rotation_matrix.h" - -namespace maliput::drake { -namespace math { - -/// This class represents a proper rigid transform between two frames which can -/// be regarded in two ways. A rigid transform describes the "pose" between two -/// frames A and B (i.e., the relative orientation and position of A to B). -/// Alternately, it can be regarded as a distance-preserving operator that can -/// rotate and/or translate a rigid body without changing its shape or size -/// (rigid) and without mirroring/reflecting the body (proper), e.g., it can add -/// one position vector to another and express the result in a particular basis -/// as `p_AoQ_A = X_AB * p_BoQ_B` (Q is any point). In many ways, this rigid -/// transform class is conceptually similar to using a homogeneous matrix as a -/// linear operator. See operator* documentation for an exception. -/// -/// The class stores a RotationMatrix that relates right-handed orthogonal -/// unit vectors Ax, Ay, Az fixed in frame A to right-handed orthogonal -/// unit vectors Bx, By, Bz fixed in frame B. -/// The class also stores a position vector from Ao (the origin of frame A) to -/// Bo (the origin of frame B). The position vector is expressed in frame A. -/// The monogram notation for the transform relating frame A to B is `X_AB`. -/// The monogram notation for the rotation matrix relating A to B is `R_AB`. -/// The monogram notation for the position vector from Ao to Bo is `p_AoBo_A`. -/// See @ref multibody_quantities for monogram notation for dynamics. -/// -/// @note This class does not store the frames associated with the transform and -/// cannot enforce correct usage of this class. For example, it makes sense to -/// multiply %RigidTransforms as `X_AB * X_BC`, but not `X_AB * X_CB`. -/// -/// @note This class is not a 4x4 transformation matrix -- even though its -/// operator*() methods act mostly like 4x4 matrix multiplication. Instead, -/// this class contains a 3x3 rotation matrix class and a 3x1 position vector. -/// To convert this to a 3x4 matrix, use GetAsMatrix34(). -/// To convert this to a 4x4 matrix, use GetAsMatrix4(). -/// To convert this to an Eigen::Isometry, use GetAsIsometry(). -/// -/// @note An isometry is sometimes regarded as synonymous with rigid transform. -/// The %RigidTransform class has important advantages over Eigen::Isometry. -/// - %RigidTransform is built on an underlying rigorous 3x3 RotationMatrix -/// class that has significant functionality for 3D orientation. -/// - In Debug builds, %RigidTransform requires a valid 3x3 rotation matrix -/// and a valid (non-NAN) position vector. Eigen::Isometry does not. -/// - %RigidTransform catches bugs that are undetected by Eigen::Isometry. -/// - %RigidTransform has additional functionality and ease-of-use, -/// resulting in shorter, easier to write, and easier to read code. -/// - The name Isometry is unfamiliar to many roboticists and dynamicists and -/// for them Isometry.linear() is (for example) a counter-intuitive method -/// name to return a rotation matrix. -/// -/// @note One of the constructors in this class provides an implicit conversion -/// from an Eigen Translation to %RigidTransform. -/// -/// @authors Paul Mitiguy (2018) Original author. -/// @authors Drake team (see https://drake.mit.edu/credits). -/// -/// @tparam_default_scalar -template -class RigidTransform { - public: - DRAKE_DEFAULT_COPY_AND_MOVE_AND_ASSIGN(RigidTransform) - - /// Constructs the %RigidTransform that corresponds to aligning the two frames - /// so unit vectors Ax = Bx, Ay = By, Az = Bz and point Ao is coincident with - /// Bo. Hence, the constructed %RigidTransform contains an identity - /// RotationMatrix and a zero position vector. - RigidTransform() { - // @internal The default RotationMatrix constructor is an identity matrix. - set_translation(Vector3::Zero()); - } - - /// Constructs a %RigidTransform from a rotation matrix and a position vector. - /// @param[in] R rotation matrix relating frames A and B (e.g., `R_AB`). - /// @param[in] p position vector from frame A's origin to frame B's origin, - /// expressed in frame A. In monogram notation p is denoted `p_AoBo_A`. - RigidTransform(const RotationMatrix& R, const Vector3& p) { set(R, p); } - - /// Constructs a %RigidTransform from a RollPitchYaw and a position vector. - /// @param[in] rpy a %RollPitchYaw which is a Space-fixed (extrinsic) X-Y-Z - /// rotation with "roll-pitch-yaw" angles `[r, p, y]` or equivalently a Body- - /// fixed (intrinsic) Z-Y-X rotation with "yaw-pitch-roll" angles `[y, p, r]`. - /// @see RotationMatrix::RotationMatrix(const RollPitchYaw&) - /// @param[in] p position vector from frame A's origin to frame B's origin, - /// expressed in frame A. In monogram notation p is denoted `p_AoBo_A`. - RigidTransform(const RollPitchYaw& rpy, const Vector3& p) - : RigidTransform(RotationMatrix(rpy), p) {} - - /// Constructs a %RigidTransform from a Quaternion and a position vector. - /// @param[in] quaternion a non-zero, finite quaternion which may or may not - /// have unit length [i.e., `quaternion.norm()` does not have to be 1]. - /// @param[in] p position vector from frame A's origin to frame B's origin, - /// expressed in frame A. In monogram notation p is denoted `p_AoBo_A`. - /// @throws std::exception in debug builds if the rotation matrix - /// that is built from `quaternion` is invalid. - /// @see RotationMatrix::RotationMatrix(const Eigen::Quaternion&) - RigidTransform(const Eigen::Quaternion& quaternion, const Vector3& p) - : RigidTransform(RotationMatrix(quaternion), p) {} - - /// Constructs a %RigidTransform from an AngleAxis and a position vector. - /// @param[in] theta_lambda an Eigen::AngleAxis whose associated axis (vector - /// direction herein called `lambda`) is non-zero and finite, but which may or - /// may not have unit length [i.e., `lambda.norm()` does not have to be 1]. - /// @param[in] p position vector from frame A's origin to frame B's origin, - /// expressed in frame A. In monogram notation p is denoted `p_AoBo_A - /// @throws std::exception in debug builds if the rotation matrix - /// that is built from `theta_lambda` is invalid. - /// @see RotationMatrix::RotationMatrix(const Eigen::AngleAxis&) - RigidTransform(const Eigen::AngleAxis& theta_lambda, const Vector3& p) - : RigidTransform(RotationMatrix(theta_lambda), p) {} - - /// Constructs a %RigidTransform with a given RotationMatrix and a zero - /// position vector. - /// @param[in] R rotation matrix relating frames A and B (e.g., `R_AB`). - explicit RigidTransform(const RotationMatrix& R) { - set(R, Vector3::Zero()); - } - - /// Constructs a %RigidTransform that contains an identity RotationMatrix and - /// a given position vector `p`. - /// @param[in] p position vector from frame A's origin to frame B's origin, - /// expressed in frame A. In monogram notation p is denoted `p_AoBo_A`. - explicit RigidTransform(const Vector3& p) { set_translation(p); } - - /// Constructs a %RigidTransform that contains an identity RotationMatrix and - /// the position vector underlying the given `translation`. - /// @param[in] translation translation-only transform that stores p_AoQ_A, the - /// position vector from frame A's origin to a point Q, expressed in frame A. - /// @note The constructed %RigidTransform `X_AAq` relates frame A to a frame - /// Aq whose basis unit vectors are aligned with Ax, Ay, Az and whose origin - /// position is located at point Q. - /// @note This constructor provides an implicit conversion from Translation to - /// %RigidTransform. - RigidTransform( // NOLINT(runtime/explicit) - const Eigen::Translation& translation) { - set_translation(translation.translation()); - } - - /// Constructs a %RigidTransform from an Eigen Isometry3. - /// @param[in] pose Isometry3 that contains an allegedly valid rotation matrix - /// `R_AB` and also contains a position vector `p_AoBo_A` from frame A's - /// origin to frame B's origin. `p_AoBo_A` must be expressed in frame A. - /// @throws std::exception in debug builds if R_AB is not a proper - /// orthonormal 3x3 rotation matrix. - /// @note No attempt is made to orthogonalize the 3x3 rotation matrix part of - /// `pose`. As needed, use RotationMatrix::ProjectToRotationMatrix(). - explicit RigidTransform(const Isometry3& pose) { SetFromIsometry3(pose); } - - /// Constructs a %RigidTransform from a 3x4 matrix whose structure is below. - /// @param[in] pose 3x4 matrix that contains an allegedly valid 3x3 rotation - /// matrix `R_AB` and also a 3x1 position vector `p_AoBo_A` (the position - /// vector from frame A's origin to frame B's origin, expressed in frame A). - /// 
-  ///  ┌                ┐
-  ///  │ R_AB  p_AoBo_A │
-  ///  └                ┘
-  ///
- /// @throws std::exception in debug builds if the `R_AB` part of `pose` is - /// not a proper orthonormal 3x3 rotation matrix. - /// @note No attempt is made to orthogonalize the 3x3 rotation matrix part of - /// `pose`. As needed, use RotationMatrix::ProjectToRotationMatrix(). - /// @exclude_from_pydrake_mkdoc{This overload is not bound in pydrake.} - explicit RigidTransform(const Eigen::Matrix pose) { - set_rotation(RotationMatrix(pose.template block<3, 3>(0, 0))); - set_translation(pose.template block<3, 1>(0, 3)); - } - - /// Constructs a %RigidTransform from a 4x4 matrix whose structure is below. - /// @param[in] pose 4x4 matrix that contains an allegedly valid 3x3 rotation - /// matrix `R_AB` and also a 3x1 position vector `p_AoBo_A` (the position - /// vector from frame A's origin to frame B's origin, expressed in frame A). - /// 
-  ///  ┌                ┐
-  ///  │ R_AB  p_AoBo_A │
-  ///  │                │
-  ///  │   0      1     │
-  ///  └                ┘
-  ///
- /// @throws std::exception in debug builds if the `R_AB` part of `pose` - /// is not a proper orthonormal 3x3 rotation matrix or if `pose` is a 4x4 - /// matrix whose final row is not `[0, 0, 0, 1]`. - /// @note No attempt is made to orthogonalize the 3x3 rotation matrix part of - /// `pose`. As needed, use RotationMatrix::ProjectToRotationMatrix(). - /// @exclude_from_pydrake_mkdoc{This overload is not bound in pydrake.} - explicit RigidTransform(const Matrix4& pose) { - MALIPUT_DRAKE_ASSERT_VOID(ThrowIfInvalidBottomRow(pose)); - set_rotation(RotationMatrix(pose.template block<3, 3>(0, 0))); - set_translation(pose.template block<3, 1>(0, 3)); - } - - /// Constructs a %RigidTransform from an appropriate Eigen expression. - /// @param[in] pose Generic Eigen matrix expression. - /// @throws std::exception if the Eigen expression in pose does not - /// resolve to a Vector3 or 3x4 matrix or 4x4 matrix or if the rotational part - /// of `pose` is not a proper orthonormal 3x3 rotation matrix or if `pose` is - /// a 4x4 matrix whose final row is not `[0, 0, 0, 1]`. - /// @note No attempt is made to orthogonalize the 3x3 rotation matrix part of - /// `pose`. As needed, use RotationMatrix::ProjectToRotationMatrix(). - /// @note This constructor prevents ambiguity that would otherwise exist for a - /// %RigidTransform constructor whose argument is an Eigen expression. - /// @code{.cc} - /// const Vector3 position(4, 5, 6); - /// const RigidTransform X1(3 * position); - /// ---------------------------------------------- - /// const RotationMatrix R(RollPitchYaw(1, 2, 3)); - /// Eigen::Matrix pose34; - /// pose34 << R.matrix(), position; - /// const RigidTransform X2(1.0 * pose34); - /// ---------------------------------------------- - /// Eigen::Matrix pose4; - /// pose4 << R.matrix(), position, - /// 0, 0, 0, 1; - /// const RigidTransform X3(pose4 * pose4); - /// @endcode - template - explicit RigidTransform(const Eigen::MatrixBase& pose) { - // TODO(Mitiguy) Consider C++ 17 if(constexpr) to specialize for each type. - const int num_rows = pose.rows(), num_cols = pose.cols(); - if (num_rows == 3 && num_cols == 1) { - // The next line cannot use set_translation(pose.cols(0)) since this - // templated class must compile for 4x4 matrices. If pose is a 4x4 matrix - // pose.cols(0) would be a 4x1 matrix --which would cause a compiler - // error since set_translation() requires a 3x1 matrix. - // Hence, the block method below must be used as it avoids Eigen static - // assertions that would otherwise cause a compiler error. In runtime, - // the line below is executed only if pose is actually a 3x1 matrix. - set_translation(pose.template block<3, 1>(0, 0)); - } else if (num_rows == 3 && num_cols == 4) { - set_rotation(RotationMatrix(pose.template block<3, 3>(0, 0))); - set_translation(pose.template block<3, 1>(0, 3)); - } else if (num_rows == 4 && num_cols == 4) { - MALIPUT_DRAKE_ASSERT_VOID(ThrowIfInvalidBottomRow(pose)); - set_rotation(RotationMatrix(pose.template block<3, 3>(0, 0))); - set_translation(pose.template block<3, 1>(0, 3)); - } else { - throw std::logic_error("Error: RigidTransform constructor argument is " - "not an Eigen expression that can resolve to" - "a Vector3 or 3x4 matrix or 4x4 matrix."); - } - } - - /// Sets `this` %RigidTransform from a RotationMatrix and a position vector. - /// @param[in] R rotation matrix relating frames A and B (e.g., `R_AB`). - /// @param[in] p position vector from frame A's origin to frame B's origin, - /// expressed in frame A. In monogram notation p is denoted `p_AoBo_A`. - void set(const RotationMatrix& R, const Vector3& p) { - set_rotation(R); - set_translation(p); - } - - /// Sets `this` %RigidTransform from an Eigen Isometry3. - /// @param[in] pose Isometry3 that contains an allegedly valid rotation matrix - /// `R_AB` and also contains a position vector `p_AoBo_A` from frame A's - /// origin to frame B's origin. `p_AoBo_A` must be expressed in frame A. - /// @throws std::exception in debug builds if R_AB is not a proper - /// orthonormal 3x3 rotation matrix. - /// @note No attempt is made to orthogonalize the 3x3 rotation matrix part of - /// `pose`. As needed, use RotationMatrix::ProjectToRotationMatrix(). - void SetFromIsometry3(const Isometry3& pose) { - set(RotationMatrix(pose.linear()), pose.translation()); - } - - /// Creates a %RigidTransform templatized on a scalar type U from a - /// %RigidTransform templatized on scalar type T. For example, - /// ``` - /// RigidTransform source = RigidTransform::Identity(); - /// RigidTransform foo = source.cast(); - /// ``` - /// @tparam U Scalar type on which the returned %RigidTransform is templated. - /// @note `RigidTransform::cast()` creates a new - /// `RigidTransform` from a `RigidTransform` but only if type `To` - /// is constructible from type `From`. This cast method works in accordance - /// with Eigen's cast method for Eigen's objects that underlie this - /// %RigidTransform. For example, Eigen currently allows cast from type - /// double to AutoDiffXd, but not vice-versa. - template - RigidTransform cast() const { - const RotationMatrix R = R_AB_.template cast(); - const Vector3 p = p_AoBo_A_.template cast(); - return RigidTransform(R, p); - } - - /// Returns the identity %RigidTransform (corresponds to coincident frames). - /// @return the %RigidTransform that corresponds to aligning the two frames so - /// unit vectors Ax = Bx, Ay = By, Az = Bz and point Ao is coincident with Bo. - /// Hence, the returned %RigidTransform contains a 3x3 identity matrix and a - /// zero position vector. - static const RigidTransform& Identity() { - // @internal This method's name was chosen to mimic Eigen's Identity(). - static const never_destroyed> kIdentity; - return kIdentity.access(); - } - - /// Returns R_AB, the rotation matrix portion of `this` %RigidTransform. - /// @retval R_AB the rotation matrix portion of `this` %RigidTransform. - const RotationMatrix& rotation() const { return R_AB_; } - - /// Sets the %RotationMatrix portion of `this` %RigidTransform. - /// @param[in] R rotation matrix relating frames A and B (e.g., `R_AB`). - void set_rotation(const RotationMatrix& R) { R_AB_ = R; } - - /// Sets the rotation part of `this` %RigidTransform from a RollPitchYaw. - /// @param[in] rpy "roll-pitch-yaw" angles. - /// @see RotationMatrix::RotationMatrix(const RollPitchYaw&) which - /// describes the parameter, preconditions, etc. - void set_rotation(const RollPitchYaw& rpy) { - set_rotation(RotationMatrix(rpy)); - } - - /// Sets the rotation part of `this` %RigidTransform from a Quaternion. - /// @param[in] quaternion a quaternion which may or may not have unit length. - /// @see RotationMatrix::RotationMatrix(const Eigen::Quaternion&) which - /// describes the parameter, preconditions, exception conditions, etc. - void set_rotation(const Eigen::Quaternion& quaternion) { - set_rotation(RotationMatrix(quaternion)); - } - - /// Sets the rotation part of `this` %RigidTransform from an AngleAxis. - /// @param[in] theta_lambda an angle `theta` (in radians) and vector `lambda`. - /// @see RotationMatrix::RotationMatrix(const Eigen::AngleAxis&) which - /// describes the parameter, preconditions, exception conditions, etc. - void set_rotation(const Eigen::AngleAxis& theta_lambda) { - set_rotation(RotationMatrix(theta_lambda)); - } - - /// Returns `p_AoBo_A`, the position vector portion of `this` %RigidTransform, - /// i.e., position vector from Ao (frame A's origin) to Bo (frame B's origin). - const Vector3& translation() const { return p_AoBo_A_; } - - /// Sets the position vector portion of `this` %RigidTransform. - /// @param[in] p position vector from Ao (frame A's origin) to Bo (frame B's - /// origin) expressed in frame A. In monogram notation p is denoted p_AoBo_A. - void set_translation(const Vector3& p) { p_AoBo_A_ = p; } - - /// Returns the 4x4 matrix associated with this %RigidTransform, i.e., X_AB. - /// 
-  ///  ┌                ┐
-  ///  │ R_AB  p_AoBo_A │
-  ///  │                │
-  ///  │   0      1     │
-  ///  └                ┘
-  ///
- Matrix4 GetAsMatrix4() const { - Matrix4 pose; - pose.template topLeftCorner<3, 3>() = rotation().matrix(); - pose.template topRightCorner<3, 1>() = translation(); - pose.row(3) = Vector4(0, 0, 0, 1); - return pose; - } - - /// Returns the 3x4 matrix associated with this %RigidTransform, i.e., X_AB. - /// 
-  ///  ┌                ┐
-  ///  │ R_AB  p_AoBo_A │
-  ///  └                ┘
-  ///
- Eigen::Matrix GetAsMatrix34() const { - Eigen::Matrix pose; - pose.template topLeftCorner<3, 3>() = rotation().matrix(); - pose.template topRightCorner<3, 1>() = translation(); - return pose; - } - - /// Returns the isometry in ℜ³ that is equivalent to a %RigidTransform. - Isometry3 GetAsIsometry3() const { - // pose.linear() returns a mutable reference to the 3x3 rotation matrix part - // of Isometry3 and pose.translation() returns a mutable reference to the - // 3x1 position vector part of the Isometry3. - Isometry3 pose; - pose.linear() = rotation().matrix(); - pose.translation() = translation(); - pose.makeAffine(); - return pose; - } - - /// Sets `this` %RigidTransform so it corresponds to aligning the two frames - /// so unit vectors Ax = Bx, Ay = By, Az = Bz and point Ao is coincident with - /// Bo. Hence, `this` %RigidTransform contains a 3x3 identity matrix and a - /// zero position vector. - const RigidTransform& SetIdentity() { - set(RotationMatrix::Identity(), Vector3::Zero()); - return *this; - } - - /// Returns `true` if `this` is exactly the identity %RigidTransform. - /// @see IsIdentityToEpsilon(). - boolean IsExactlyIdentity() const { - const boolean is_position_zero = (translation() == Vector3::Zero()); - return is_position_zero && rotation().IsExactlyIdentity(); - } - - /// Return true if `this` is within tolerance of the identity %RigidTransform. - /// @returns `true` if the RotationMatrix portion of `this` satisfies - /// RotationMatrix::IsIdentityToInternalTolerance() and if the position vector - /// portion of `this` is equal to zero vector within `translation_tolerance`. - /// @param[in] translation_tolerance a non-negative number. One way to choose - /// `translation_tolerance` is to multiply a characteristic length - /// (e.g., the magnitude of a characteristic position vector) by an epsilon - /// (e.g., RotationMatrix::get_internal_tolerance_for_orthonormality()). - /// @see IsExactlyIdentity(). - boolean IsIdentityToEpsilon(double translation_tolerance) const { - const T max_component = translation().template lpNorm(); - return max_component <= translation_tolerance && - rotation().IsIdentityToInternalTolerance(); - } - - /// Returns X_BA = X_AB⁻¹, the inverse of `this` %RigidTransform. - /// @note The inverse of %RigidTransform X_AB is X_BA, which contains the - /// rotation matrix R_BA = R_AB⁻¹ = R_ABᵀ and the position vector `p_BoAo_B_` - /// (position from B's origin Bo to A's origin Ao, expressed in frame B). - /// @note: The square-root of a %RigidTransform's condition number is roughly - /// the magnitude of the position vector. The accuracy of the calculation for - /// the inverse of a %RigidTransform drops off with the sqrt condition number. - RigidTransform inverse() const { - // @internal This method's name was chosen to mimic Eigen's inverse(). - const RotationMatrix R_BA = R_AB_.inverse(); - return RigidTransform(R_BA, R_BA * (-p_AoBo_A_)); - } - - /// In-place multiply of `this` %RigidTransform `X_AB` by `other` - /// %RigidTransform `X_BC`. - /// @param[in] other %RigidTransform that post-multiplies `this`. - /// @returns `this` %RigidTransform which has been multiplied by `other`. - /// On return, `this = X_AC`, where `X_AC = X_AB * X_BC`. - RigidTransform& operator*=(const RigidTransform& other) { - if constexpr (std::is_same_v) { - internal::ComposeXX(*this, other, this); - } else { - p_AoBo_A_ = *this * other.translation(); - R_AB_ *= other.rotation(); - } - return *this; - } - - /// Multiplies `this` %RigidTransform `X_AB` by the `other` %RigidTransform - /// `X_BC` and returns the %RigidTransform `X_AC = X_AB * X_BC`. - RigidTransform operator*(const RigidTransform& other) const { - RigidTransform X_AC(internal::DoNotInitializeMemberFields{}); - if constexpr (std::is_same_v) { - internal::ComposeXX(*this, other, &X_AC); - } else { - X_AC.set(rotation() * other.rotation(), *this * other.translation()); - } - return X_AC; - } - - /// Calculates the product of `this` inverted and another %RigidTransform. - /// If you consider `this` to be the transform X_AB, and `other` to be - /// X_AC, then this method returns X_BC = X_AB⁻¹ * X_AC. For T==double, this - /// method can be _much_ faster than inverting first and then performing the - /// composition, because it can take advantage of the special structure of - /// a rigid transform to avoid unnecessary memory and floating point - /// operations. On some platforms it can use SIMD instructions for further - /// speedups. - /// @param[in] other %RigidTransform that post-multiplies `this` inverted. - /// @retval X_BC where X_BC = this⁻¹ * other. - /// @note It is possible (albeit improbable) to create an invalid rigid - /// transform by accumulating round-off error with a large number of - /// multiplies. - RigidTransform InvertAndCompose(const RigidTransform& other) const { - const RigidTransform& X_AC = other; // Nicer name. - RigidTransform X_BC(internal::DoNotInitializeMemberFields{}); - if constexpr (std::is_same_v) { - internal::ComposeXinvX(*this, X_AC, &X_BC); - } else { - const RigidTransform X_BA = inverse(); - X_BC = X_BA * X_AC; - } - return X_BC; - } - - /// Multiplies `this` %RigidTransform `X_AB` by the translation-only transform - /// `X_BBq` and returns the %RigidTransform `X_ABq = X_AB * X_BBq`. - /// @note The rotation matrix in the returned %RigidTransform `X_ABq` is equal - /// to the rotation matrix in `X_AB`. `X_ABq` and `X_AB` only differ by - /// origin location. - RigidTransform operator*(const Eigen::Translation& X_BBq) const { - const RigidTransform& X_AB = *this; - const Vector3 p_ABq_A = X_AB * X_BBq.translation(); - return RigidTransform(rotation(), p_ABq_A); - } - - /// Multiplies the translation-only transform `X_AAq` by the %RigidTransform - /// `X_AqB` and returns the %RigidTransform `X_AB = X_AAq * X_AqB`. - /// @note The rotation matrix in the returned %RigidTransform `X_AB` is equal - /// to the rotation matrix in `X_AqB`. `X_AB` and `X_AqB` only differ by - /// origin location. - friend RigidTransform operator*(const Eigen::Translation& X_AAq, - const RigidTransform& X_AqB) { - const Vector3& p_AAq_A = X_AAq.translation(); - const Vector3& p_AqB_A = X_AqB.translation(); - const Vector3& p_AB_A = p_AAq_A + p_AqB_A; - const RotationMatrix& R_AB = X_AqB.rotation(); - return RigidTransform(R_AB, p_AB_A); - } - - /// Multiplies `this` %RigidTransform `X_AB` by the position vector - /// `p_BoQ_B` which is from Bo (B's origin) to an arbitrary point Q. - /// @param[in] p_BoQ_B position vector from Bo to Q, expressed in frame B. - /// @retval p_AoQ_A position vector from Ao to Q, expressed in frame A. - Vector3 operator*(const Vector3& p_BoQ_B) const { - return p_AoBo_A_ + R_AB_ * p_BoQ_B; - } - - /// Multiplies `this` %RigidTransform `X_AB` by the n position vectors - /// `p_BoQ1_B` ... `p_BoQn_B`, where `p_BoQi_B` is the iᵗʰ position vector - /// from Bo (frame B's origin) to an arbitrary point Qi, expressed in frame B. - /// @param[in] p_BoQ_B `3 x n` matrix with n position vectors `p_BoQi_B` or - /// an expression that resolves to a `3 x n` matrix of position vectors. - /// @retval p_AoQ_A `3 x n` matrix with n position vectors `p_AoQi_A`, i.e., n - /// position vectors from Ao (frame A's origin) to Qi, expressed in frame A. - /// Specifically, this operator* is defined so that `X_AB * p_BoQ_B` returns - /// `p_AoQ_A = p_AoBo_A + R_AB * p_BoQ_B`, where - /// `p_AoBo_A` is the position vector from Ao to Bo expressed in A and - /// `R_AB` is the rotation matrix relating the orientation of frames A and B. - /// @note As needed, use parentheses. This operator* is not associative. - /// To see this, let `p = p_AoBo_A`, `q = p_BoQ_B` and note - /// (X_AB * q) * 7 = (p + R_AB * q) * 7 ≠ X_AB * (q * 7) = p + R_AB * (q * 7). - /// @code{.cc} - /// const RollPitchYaw rpy(0.1, 0.2, 0.3); - /// const RigidTransform X_AB(rpy, Vector3d(1, 2, 3)); - /// Eigen::Matrix p_BoQ_B; - /// p_BoQ_B.col(0) = Vector3d(4, 5, 6); - /// p_BoQ_B.col(1) = Vector3d(9, 8, 7); - /// const Eigen::Matrix p_AoQ_A = X_AB * p_BoQ_B; - /// @endcode - template - Eigen::Matrix - operator*(const Eigen::MatrixBase& p_BoQ_B) const { - if (p_BoQ_B.rows() != 3) { - throw std::logic_error( - "Error: Inner dimension for matrix multiplication is not 3."); - } - // Express position vectors in terms of frame A as p_BoQ_A = R_AB * p_BoQ_B. - const RotationMatrix &R_AB = rotation(); - const Eigen::Matrix - p_BoQ_A = R_AB * p_BoQ_B; - - // Reserve space (on stack or heap) to store the result. - const int number_of_position_vectors = p_BoQ_B.cols(); - Eigen::Matrix - p_AoQ_A(3, number_of_position_vectors); - - // Create each returned position vector as p_AoQi_A = p_AoBo_A + p_BoQi_A. - for (int i = 0; i < number_of_position_vectors; ++i) - p_AoQ_A.col(i) = translation() + p_BoQ_A.col(i); - - return p_AoQ_A; - } - - /// Compares each element of `this` to the corresponding element of `other` - /// to check if they are the same to within a specified `tolerance`. - /// @param[in] other %RigidTransform to compare to `this`. - /// @param[in] tolerance maximum allowable absolute difference between the - /// elements in `this` and `other`. - /// @returns `true` if `‖this.matrix() - other.matrix()‖∞ <= tolerance`. - /// @note Consider scaling tolerance with the largest of magA and magB, where - /// magA and magB denoted the magnitudes of `this` position vector and `other` - /// position vectors, respectively. - boolean IsNearlyEqualTo(const RigidTransform& other, - double tolerance) const { - return GetMaximumAbsoluteDifference(other) <= tolerance; - } - - /// Returns true if `this` is exactly equal to `other`. - /// @param[in] other %RigidTransform to compare to `this`. - /// @returns `true` if each element of `this` is exactly equal to the - /// corresponding element of `other`. - boolean IsExactlyEqualTo(const RigidTransform& other) const { - return rotation().IsExactlyEqualTo(other.rotation()) && - translation() == other.translation(); - } - - /// Computes the infinity norm of `this` - `other` (i.e., the maximum absolute - /// value of the difference between the elements of `this` and `other`). - /// @param[in] other %RigidTransform to subtract from `this`. - /// @returns ‖`this` - `other`‖∞ - T GetMaximumAbsoluteDifference(const RigidTransform& other) const { - const Eigen::Matrix diff = GetAsMatrix34() - other.GetAsMatrix34(); - return diff.template lpNorm(); - } - - /// Returns the maximum absolute value of the difference in the position - /// vectors (translation) in `this` and `other`. In other words, returns - /// the infinity norm of the difference in the position vectors. - /// @param[in] other %RigidTransform whose position vector is subtracted from - /// the position vector in `this`. - T GetMaximumAbsoluteTranslationDifference( - const RigidTransform& other) const { - const Vector3 p_difference = translation() - other.translation(); - return p_difference.template lpNorm(); - } - - private: - // Make RigidTransform templatized on any typename U be a friend of a - // %RigidTransform templatized on any other typename T. This is needed for the - // method RigidTransform::cast() to be able to use the required private - // constructor. - template - friend class RigidTransform; - - // Declares the allowable tolerance (small multiplier of double-precision - // epsilon) used to check whether or not a matrix is homogeneous. - static constexpr double kInternalToleranceForHomogeneousCheck{ - 4 * std::numeric_limits::epsilon() }; - - // Constructs a RigidTransform without initializing the underlying 3x4 matrix. - explicit RigidTransform(internal::DoNotInitializeMemberFields) - : R_AB_{internal::DoNotInitializeMemberFields{}} {} - - // Throw an exception if the bottom row of a 4x4 matrix is not [0, 0, 0, 1]. - // Note: To allow this method to be used with other %RigidTransform methods - // that use Eigen expressions (as distinct from Eigen types) - // the `pose` argument is an Eigen::MatrixBase (not a Matrix4 type). - template - static void ThrowIfInvalidBottomRow(const Eigen::MatrixBase& pose) { - const int num_rows = pose.rows(), num_cols = pose.cols(); - MALIPUT_DRAKE_DEMAND(num_rows == 4 && num_cols == 4); - if (pose(3, 0) != 0 || pose(3, 1) != 0 || - pose(3, 2) != 0 || pose(3, 3) != 1) { - throw std::logic_error(fmt::format( - "Error: Bottom row of 4x4 matrix differs from [0, 0, 0, 1]")); - } - } - - // Rotation matrix relating two frames, e.g. frame A and frame B. - // The default constructor for R_AB_ is an identity matrix. - RotationMatrix R_AB_; - - // Position vector from A's origin Ao to B's origin Bo, expressed in A. - Vector3 p_AoBo_A_; -}; - -// To enable low-level optimizations we insist that RigidTransform is -// packed into 12 consecutive doubles, with a 3x3 RotationMatrix in -// the first 9 doubles and a translation vector in the last 3 doubles. -// A unit test verifies this memory layout for a RigidTransform. -// Note: The C++ standard guarantees that non-static members of a class appear -// in memory in the same order as they are declared. Implementation alignment -// requirements can cause an alignment gap in memory between adjacent members. -static_assert(sizeof(RigidTransform) == 12 * sizeof(double), - "Low-level optimizations depend on RigidTransform being " - "stored as 12 sequential doubles in memory."); - -/// Stream insertion operator to write an instance of RigidTransform into a -/// `std::ostream`. Especially useful for debugging. -/// @relates RigidTransform. -template -std::ostream& operator<<(std::ostream& out, const RigidTransform& X); - -/// Abbreviation (alias/typedef) for a RigidTransform double scalar type. -/// @relates RigidTransform -using RigidTransformd = RigidTransform; - -} // namespace math -} // namespace maliput::drake - -DRAKE_DECLARE_CLASS_TEMPLATE_INSTANTIATIONS_ON_DEFAULT_SCALARS( - class ::maliput::drake::math::RigidTransform) diff --git a/include/maliput/drake/math/roll_pitch_yaw.h b/include/maliput/drake/math/roll_pitch_yaw.h deleted file mode 100644 index 5d742f4..0000000 --- a/include/maliput/drake/math/roll_pitch_yaw.h +++ /dev/null @@ -1,679 +0,0 @@ -#pragma once - -#include -#include - -#include -#include - -#include "maliput/drake/common/default_scalars.h" -#include "maliput/drake/common/drake_assert.h" -#include "maliput/drake/common/drake_bool.h" -#include "maliput/drake/common/drake_copyable.h" -#include "maliput/drake/common/eigen_types.h" - -namespace maliput::drake { -namespace math { - -template -class RotationMatrix; - -// TODO(@mitiguy) Add Sherm/Goldstein's way to visualize rotation sequences. -/// This class represents the orientation between two arbitrary frames A and D -/// associated with a Space-fixed (extrinsic) X-Y-Z rotation by "roll-pitch-yaw" -/// angles `[r, p, y]`, which is equivalent to a Body-fixed (intrinsic) Z-Y-X -/// rotation by "yaw-pitch-roll" angles `[y, p, r]`. The rotation matrix `R_AD` -/// associated with this roll-pitch-yaw `[r, p, y]` rotation sequence is equal -/// to the matrix multiplication shown below. -/// ``` -/// ⎡cos(y) -sin(y) 0⎤ ⎡ cos(p) 0 sin(p)⎤ ⎡1 0 0 ⎤ -/// R_AD = ⎢sin(y) cos(y) 0⎥ * ⎢ 0 1 0 ⎥ * ⎢0 cos(r) -sin(r)⎥ -/// ⎣ 0 0 1⎦ ⎣-sin(p) 0 cos(p)⎦ ⎣0 sin(r) cos(r)⎦ -/// = R_AB * R_BC * R_CD -/// ``` -/// @note In this discussion, A is the Space frame and D is the Body frame. -/// One way to visualize this rotation sequence is by introducing intermediate -/// frames B and C (useful constructs to understand this rotation sequence). -/// Initially, the frames are aligned so `Di = Ci = Bi = Ai (i = x, y, z)` -/// where Dx, Dy, Dz and Ax, Ay, Az are orthogonal unit vectors fixed in frames -/// D and A respectively. Similarly for Bx, By, Bz and Cx, Cy, Cz in frame B, C. -/// Then D is subjected to successive right-handed rotations relative to A. -/// @li 1st rotation R_CD: %Frame D rotates relative to frames C, B, A by a -/// roll angle `r` about `Dx = Cx`. Note: D and C are no longer aligned. -/// @li 2nd rotation R_BC: Frames D, C (collectively -- as if welded together) -/// rotate relative to frame B, A by a pitch angle `p` about `Cy = By`. -/// Note: C and B are no longer aligned. -/// @li 3rd rotation R_AB: Frames D, C, B (collectively -- as if welded) -/// rotate relative to frame A by a roll angle `y` about `Bz = Az`. -/// Note: B and A are no longer aligned. -/// The monogram notation for the rotation matrix relating A to D is `R_AD`. -/// -/// @see @ref multibody_quantities for monogram notation for dynamics and -/// @ref orientation_discussion "a discussion on rotation matrices". -/// -/// @note This class does not store the frames associated with this rotation -/// sequence. -/// -/// @tparam_default_scalar -template -class RollPitchYaw { - public: - DRAKE_DEFAULT_COPY_AND_MOVE_AND_ASSIGN(RollPitchYaw) - - /// Constructs a %RollPitchYaw from a 3x1 array of angles. - /// @param[in] rpy 3x1 array with roll, pitch, yaw angles (units of radians). - /// @throws std::exception in debug builds if !IsValid(rpy). - explicit RollPitchYaw(const Vector3& rpy) { set(rpy); } - - /// Constructs a %RollPitchYaw from roll, pitch, yaw angles (radian units). - /// @param[in] roll x-directed angle in SpaceXYZ rotation sequence. - /// @param[in] pitch y-directed angle in SpaceXYZ rotation sequence. - /// @param[in] yaw z-directed angle in SpaceXYZ rotation sequence. - /// @throws std::exception in debug builds if - /// !IsValid(Vector3(roll, pitch, yaw)). - RollPitchYaw(const T& roll, const T& pitch, const T& yaw) { - set(roll, pitch, yaw); - } - - /// Uses a %RotationMatrix to construct a %RollPitchYaw with - /// roll-pitch-yaw angles `[r, p, y]` in the range - /// `-π <= r <= π`, `-π/2 <= p <= π/2`, `-π <= y <= π`. - /// @param[in] R a %RotationMatrix. - /// @note This new high-accuracy algorithm avoids numerical round-off issues - /// encountered by some algorithms when pitch is within 1E-6 of π/2 or -π/2. - explicit RollPitchYaw(const RotationMatrix& R) { - SetFromRotationMatrix(R); - } - - /// Uses a %Quaternion to construct a %RollPitchYaw with - /// roll-pitch-yaw angles `[r, p, y]` in the range - /// `-π <= r <= π`, `-π/2 <= p <= π/2`, `-π <= y <= π`. - /// @param[in] quaternion unit %Quaternion. - /// @note This new high-accuracy algorithm avoids numerical round-off issues - /// encountered by some algorithms when pitch is within 1E-6 of π/2 or -π/2. - /// @throws std::exception in debug builds if !IsValid(rpy). - explicit RollPitchYaw(const Eigen::Quaternion& quaternion) { - SetFromQuaternion(quaternion); - } - - /// Sets `this` %RollPitchYaw from a 3x1 array of angles. - /// @param[in] rpy 3x1 array with roll, pitch, yaw angles (units of radians). - /// @throws std::exception in debug builds if !IsValid(rpy). - RollPitchYaw& set(const Vector3& rpy) { - return SetOrThrowIfNotValidInDebugBuild(rpy); - } - - /// Sets `this` %RollPitchYaw from roll, pitch, yaw angles (units of radians). - /// @param[in] roll x-directed angle in SpaceXYZ rotation sequence. - /// @param[in] pitch y-directed angle in SpaceXYZ rotation sequence. - /// @param[in] yaw z-directed angle in SpaceXYZ rotation sequence. - /// @throws std::exception in debug builds if - /// !IsValid(Vector3(roll, pitch, yaw)). - RollPitchYaw& set(const T& roll, const T& pitch, const T& yaw) { - return set(Vector3(roll, pitch, yaw)); - } - - /// Uses a %Quaternion to set `this` %RollPitchYaw with - /// roll-pitch-yaw angles `[r, p, y]` in the range - /// `-π <= r <= π`, `-π/2 <= p <= π/2`, `-π <= y <= π`. - /// @param[in] quaternion unit %Quaternion. - /// @note This new high-accuracy algorithm avoids numerical round-off issues - /// encountered by some algorithms when pitch is within 1E-6 of π/2 or -π/2. - /// @throws std::exception in debug builds if !IsValid(rpy). - void SetFromQuaternion(const Eigen::Quaternion& quaternion); - - /// Uses a high-accuracy efficient algorithm to set the roll-pitch-yaw - /// angles `[r, p, y]` that underlie `this` @RollPitchYaw, even when the pitch - /// angle p is very near a singularity (when p is within 1E-6 of π/2 or -π/2). - /// After calling this method, the underlying roll-pitch-yaw `[r, p, y]` has - /// range `-π <= r <= π`, `-π/2 <= p <= π/2`, `-π <= y <= π`. - /// @param[in] R a %RotationMatrix. - /// @note This high-accuracy algorithm was invented at TRI in October 2016 and - /// avoids numerical round-off issues encountered by some algorithms when - /// pitch is within 1E-6 of π/2 or -π/2. - void SetFromRotationMatrix(const RotationMatrix& R); - - /// Returns the Vector3 underlying `this` %RollPitchYaw. - const Vector3& vector() const { return roll_pitch_yaw_; } - - /// Returns the roll-angle underlying `this` %RollPitchYaw. - const T& roll_angle() const { return roll_pitch_yaw_(0); } - - /// Returns the pitch-angle underlying `this` %RollPitchYaw. - const T& pitch_angle() const { return roll_pitch_yaw_(1); } - - /// Returns the yaw-angle underlying `this` %RollPitchYaw. - const T& yaw_angle() const { return roll_pitch_yaw_(2); } - - /// Set the roll-angle underlying `this` %RollPitchYaw. - /// @param[in] r roll angle (in units of radians). - void set_roll_angle(const T& r) { roll_pitch_yaw_(0) = r; } - - /// Set the pitch-angle underlying `this` %RollPitchYaw. - /// @param[in] p pitch angle (in units of radians). - void set_pitch_angle(const T& p) { roll_pitch_yaw_(1) = p; } - - /// Set the yaw-angle underlying `this` %RollPitchYaw. - /// @param[in] y yaw angle (in units of radians). - void set_yaw_angle(const T& y) { roll_pitch_yaw_(2) = y; } - - /// Returns a quaternion representation of `this` %RollPitchYaw. - Eigen::Quaternion ToQuaternion() const { - using std::cos; - using std::sin; - const T q0Half = roll_pitch_yaw_(0) / 2; - const T q1Half = roll_pitch_yaw_(1) / 2; - const T q2Half = roll_pitch_yaw_(2) / 2; - const T c0 = cos(q0Half), s0 = sin(q0Half); - const T c1 = cos(q1Half), s1 = sin(q1Half); - const T c2 = cos(q2Half), s2 = sin(q2Half); - const T c1_c2 = c1 * c2, s1_c2 = s1 * c2; - const T s1_s2 = s1 * s2, c1_s2 = c1 * s2; - const T w = c0 * c1_c2 + s0 * s1_s2; - const T x = s0 * c1_c2 - c0 * s1_s2; - const T y = c0 * s1_c2 + s0 * c1_s2; - const T z = c0 * c1_s2 - s0 * s1_c2; - return Eigen::Quaternion(w, x, y, z); - } - - /// Returns the RotationMatrix representation of `this` %RollPitchYaw. - RotationMatrix ToRotationMatrix() const; - - /// Returns the 3x3 matrix representation of the %RotationMatrix that - /// corresponds to `this` %RollPitchYaw. This is a convenient "sugar" method - /// that is exactly equivalent to RotationMatrix(rpy).ToMatrix3(). - Matrix3 ToMatrix3ViaRotationMatrix() const { - return ToRotationMatrix().matrix(); - } - - /// Compares each element of `this` to the corresponding element of `other` - /// to check if they are the same to within a specified `tolerance`. - /// @param[in] other %RollPitchYaw to compare to `this`. - /// @param[in] tolerance maximum allowable absolute difference between the - /// matrix elements in `this` and `other`. - /// @returns `true` if `‖this - other‖∞ <= tolerance`. - boolean IsNearlyEqualTo( - const RollPitchYaw& other, double tolerance) const { - const Vector3 difference = vector() - other.vector(); - return difference.template lpNorm() <= tolerance; - } - - /// Compares each element of the %RotationMatrix R1 produced by `this` to the - /// corresponding element of the %RotationMatrix R2 produced by `other` to - /// check if they are the same to within a specified `tolerance`. - /// @param[in] other %RollPitchYaw to compare to `this`. - /// @param[in] tolerance maximum allowable absolute difference between R1, R2. - /// @returns `true` if `‖R1 - R2‖∞ <= tolerance`. - boolean IsNearlySameOrientation(const RollPitchYaw& other, - double tolerance) const; - - /// Returns true if roll-pitch-yaw angles `[r, p, y]` are in the range - /// `-π <= r <= π`, `-π/2 <= p <= π/2`, `-π <= y <= π`. - boolean IsRollPitchYawInCanonicalRange() const { - const T& r = roll_angle(); - const T& p = pitch_angle(); - const T& y = yaw_angle(); - return (-M_PI <= r && r <= M_PI) && (-M_PI / 2 <= p && p <= M_PI / 2) && - (-M_PI <= y && y <= M_PI); - } - - /// Returns true if the pitch-angle `p` is close to gimbal-lock, which means - /// `cos(p) ≈ 0` or `p ≈ (n*π + π/2)` where `n = 0, ±1, ±2, ...`. - /// More specifically, returns true if `abs(cos_pitch_angle)` is less than an - /// internally-defined tolerance of gimbal-lock. - /// @param[in] cos_pitch_angle cosine of the pitch angle, i.e., `cos(p)`. - /// @note Pitch-angles close to gimbal-lock can cause problems with numerical - /// precision and numerical integration. - static boolean DoesCosPitchAngleViolateGimbalLockTolerance( - const T& cos_pitch_angle) { - using std::abs; - return abs(cos_pitch_angle) < kGimbalLockToleranceCosPitchAngle; - } - - /// Returns true if the pitch-angle `p` is within an internally-defined - /// tolerance of gimbal-lock. In other words, this method returns true if - /// `p ≈ (n*π + π/2)` where `n = 0, ±1, ±2, ...`. - /// @note To improve efficiency when cos(pitch_angle()) is already calculated, - /// instead use the function DoesCosPitchAngleViolateGimbalLockTolerance(). - /// @see DoesCosPitchAngleViolateGimbalLockTolerance() - boolean DoesPitchAngleViolateGimbalLockTolerance() const { - using std::cos; - return DoesCosPitchAngleViolateGimbalLockTolerance(cos(pitch_angle())); - } - - /// Returns the internally-defined allowable closeness (in radians) of the - /// pitch angle `p` to gimbal-lock, i.e., the allowable proximity of `p` to - /// `(n*π + π/2)` where `n = 0, ±1, ±2, ...`. - static double GimbalLockPitchAngleTolerance() { - return M_PI_2 - std::acos(kGimbalLockToleranceCosPitchAngle); - } - - /// Returns true if `rpy` contains valid roll, pitch, yaw angles. - /// @param[in] rpy allegedly valid roll, pitch, yaw angles. - /// @note an angle is invalid if it is NaN or infinite. - static boolean IsValid(const Vector3& rpy) { - using std::isfinite; - return isfinite(rpy[0]) && isfinite(rpy[1]) && isfinite(rpy[2]); - } - - /// Forms Ṙ, the ordinary derivative of the %RotationMatrix `R` with respect - /// to an independent variable `t` (`t` usually denotes time) and `R` is the - /// %RotationMatrix formed by `this` %RollPitchYaw. The roll-pitch-yaw angles - /// r, p, y are regarded as functions of `t` [i.e., r(t), p(t), y(t)]. - /// @param[in] rpyDt Ordinary derivative of rpy with respect to an independent - /// variable `t` (`t` usually denotes time, but not necessarily). - /// @returns Ṙ, the ordinary derivative of `R` with respect to `t`, calculated - /// as Ṙ = ∂R/∂r * ṙ + ∂R/∂p * ṗ + ∂R/∂y * ẏ. In other words, the returned - /// (i, j) element is ∂Rij/∂r * ṙ + ∂Rij/∂p * ṗ + ∂Rij/∂y * ẏ. - Matrix3 CalcRotationMatrixDt(const Vector3& rpyDt) const { - // For the rotation matrix R generated by `this` RollPitchYaw, calculate the - // partial derivatives of R with respect to roll `r`, pitch `p` and yaw `y`. - Matrix3 R_r, R_p, R_y; // ∂R/∂r, ∂R/∂p, ∂R/∂y - CalcRotationMatrixDrDpDy(&R_r, &R_p, &R_y); - - // When rotation matrix R is regarded as an implicit function of t as - // R(r(t), p(t), y(t)), the ordinary derivative of R with respect to t is - // Ṙ = ∂R/∂r * ṙ + ∂R/∂p * ṗ + ∂R/∂y * ẏ - const T rDt = rpyDt(0), pDt = rpyDt(1), yDt = rpyDt(2); - return R_r * rDt + R_p * pDt + R_y * yDt; - } - - /// Calculates angular velocity from `this` %RollPitchYaw whose roll-pitch-yaw - /// angles `[r; p; y]` relate the orientation of two generic frames A and D. - /// @param[in] rpyDt Time-derivative of `[r; p; y]`, i.e., `[ṙ; ṗ; ẏ]`. - /// @returns w_AD_A, frame D's angular velocity in frame A, expressed in A. - Vector3 CalcAngularVelocityInParentFromRpyDt( - const Vector3& rpyDt) const { - // Get the 3x3 coefficient matrix M that contains the partial derivatives of - // w_AD_A with respect to ṙ, ṗ, ẏ. In other words, `w_AD_A = M * rpyDt`. - // TODO(Mitiguy) Improve speed -- last column of M is (0, 0, 1). - const Matrix3 M = CalcMatrixRelatingAngularVelocityInParentToRpyDt(); - return M * rpyDt; - } - - /// Calculates angular velocity from `this` %RollPitchYaw whose roll-pitch-yaw - /// angles `[r; p; y]` relate the orientation of two generic frames A and D. - /// @param[in] rpyDt Time-derivative of `[r; p; y]`, i.e., `[ṙ; ṗ; ẏ]`. - /// @returns w_AD_D, frame D's angular velocity in frame A, expressed in D. - Vector3 CalcAngularVelocityInChildFromRpyDt( - const Vector3& rpyDt) const { - // Get the 3x3 coefficient matrix M that contains the partial derivatives of - // w_AD_D with respect to ṙ, ṗ, ẏ. In other words, `w_AD_D = M * rpyDt`. - // TODO(Mitiguy) Improve speed -- first column of M is (1, 0, 0). - const Matrix3 M = CalcMatrixRelatingAngularVelocityInChildToRpyDt(); - return M * rpyDt; - } - - /// Uses angular velocity to compute the 1ˢᵗ time-derivative of `this` - /// %RollPitchYaw whose angles `[r; p; y]` orient two generic frames A and D. - /// @param[in] w_AD_A, frame D's angular velocity in frame A, expressed in A. - /// @returns `[ṙ; ṗ; ẏ]`, the 1ˢᵗ time-derivative of `this` %RollPitchYaw. - /// @throws std::exception if `cos(p) ≈ 0` (`p` is near gimbal-lock). - /// @note This method has a divide-by-zero error (singularity) when the cosine - /// of the pitch angle `p` is zero [i.e., `cos(p) = 0`]. This problem (called - /// "gimbal lock") occurs when `p = n π + π / 2`, where n is any integer. - /// There are associated precision problems (inaccuracies) in the neighborhood - /// of these pitch angles, i.e., when `cos(p) ≈ 0`. - Vector3 CalcRpyDtFromAngularVelocityInParent( - const Vector3& w_AD_A) const { - // Get the 3x3 M matrix that contains the partial derivatives of `[ṙ, ṗ, ẏ]` - // with respect to `[wx; wy; wz]ₐ` (which is w_AD_A expressed in A). - // In other words, `rpyDt = M * w_AD_A`. - // TODO(Mitiguy) Improve speed -- last column of M is (0, 0, 1). - // TODO(Mitiguy) Improve accuracy when `cos(p) ≈ 0`. - const Matrix3 M = CalcMatrixRelatingRpyDtToAngularVelocityInParent( - __func__, __FILE__, __LINE__); - return M * w_AD_A; - } - - /// For `this` %RollPitchYaw with roll-pitch-yaw angles `[r; p; y]` which - /// relate the orientation of two generic frames A and D, returns the 3x3 - /// matrix M that contains the partial derivatives of [ṙ, ṗ, ẏ] with respect - /// to `[wx; wy; wz]ₐ` (which is w_AD_A expressed in A). - /// In other words, `rpyDt = M * w_AD_A`. - /// @param[in] function_name name of the calling function/method. - /// @throws std::exception if `cos(p) ≈ 0` (`p` is near gimbal-lock). - /// @note This method has a divide-by-zero error (singularity) when the cosine - /// of the pitch angle `p` is zero [i.e., `cos(p) = 0`]. This problem (called - /// "gimbal lock") occurs when `p = n π + π / 2`, where n is any integer. - /// There are associated precision problems (inaccuracies) in the neighborhood - /// of these pitch angles, i.e., when `cos(p) ≈ 0`. - const Matrix3 CalcMatrixRelatingRpyDtToAngularVelocityInParent() const { - const Matrix3 M = CalcMatrixRelatingRpyDtToAngularVelocityInParent( - __func__, __FILE__, __LINE__); - return M; - } - - /// Uses angular acceleration to compute the 2ⁿᵈ time-derivative of `this` - /// %RollPitchYaw whose angles `[r; p; y]` orient two generic frames A and D. - /// @param[in] rpyDt time-derivative of `[r; p; y]`, i.e., `[ṙ; ṗ; ẏ]`. - /// @param[in] alpha_AD_A, frame D's angular acceleration in frame A, - /// expressed in frame A. - /// @returns `[r̈, p̈, ÿ]`, the 2ⁿᵈ time-derivative of `this` %RollPitchYaw. - /// @throws std::exception if `cos(p) ≈ 0` (`p` is near gimbal-lock). - /// @note This method has a divide-by-zero error (singularity) when the cosine - /// of the pitch angle `p` is zero [i.e., `cos(p) = 0`]. This problem (called - /// "gimbal lock") occurs when `p = n π + π / 2`, where n is any integer. - /// There are associated precision problems (inaccuracies) in the neighborhood - /// of these pitch angles, i.e., when `cos(p) ≈ 0`. - Vector3 CalcRpyDDtFromRpyDtAndAngularAccelInParent( - const Vector3& rpyDt, const Vector3& alpha_AD_A) const { - // TODO(Mitiguy) Improve accuracy when `cos(p) ≈ 0`. - // TODO(Mitiguy) Improve speed: The last column of M is (0, 0, 1), the last - // column of MDt is (0, 0, 1) and there are repeated sin/cos calculations. - const Matrix3 Minv = CalcMatrixRelatingRpyDtToAngularVelocityInParent( - __func__, __FILE__, __LINE__); - const Matrix3 MDt = - CalcDtMatrixRelatingAngularVelocityInParentToRpyDt(rpyDt); - return Minv * (alpha_AD_A - MDt * rpyDt); - } - - /// Uses angular acceleration to compute the 2ⁿᵈ time-derivative of `this` - /// %RollPitchYaw whose angles `[r; p; y]` orient two generic frames A and D. - /// @param[in] rpyDt time-derivative of `[r; p; y]`, i.e., `[ṙ; ṗ; ẏ]`. - /// @param[in] alpha_AD_D, frame D's angular acceleration in frame A, - /// expressed in frame D. - /// @returns `[r̈, p̈, ÿ]`, the 2ⁿᵈ time-derivative of `this` %RollPitchYaw. - /// @throws std::exception if `cos(p) ≈ 0` (`p` is near gimbal-lock). - /// @note This method has a divide-by-zero error (singularity) when the cosine - /// of the pitch angle `p` is zero [i.e., `cos(p) = 0`]. This problem (called - /// "gimbal lock") occurs when `p = n π + π / 2`, where n is any integer. - /// There are associated precision problems (inaccuracies) in the neighborhood - /// of these pitch angles, i.e., when `cos(p) ≈ 0`. - Vector3 CalcRpyDDtFromAngularAccelInChild( - const Vector3& rpyDt, const Vector3& alpha_AD_D) const { - const T& r = roll_angle(); - const T& p = pitch_angle(); - using std::sin; - using std::cos; - const T sr = sin(r), cr = cos(r); - const T sp = sin(p), cp = cos(p); - if (DoesCosPitchAngleViolateGimbalLockTolerance(cp)) { - ThrowPitchAngleViolatesGimbalLockTolerance(__func__, __FILE__, __LINE__, - p); - } - const T one_over_cp = T(1)/cp; - const T cr_over_cp = cr * one_over_cp; - const T sr_over_cp = sr * one_over_cp; - // clang-format on - Matrix3 M; - M << T(1), sr_over_cp * sp, cr_over_cp * sp, - T(0), cr, -sr, - T(0), sr_over_cp, cr_over_cp; - // clang-format off - - // Remainder terms (terms not multiplying α). - const T tanp = sp * one_over_cp; - const T rDt = rpyDt(0), pDt = rpyDt(1), yDt = rpyDt(2); - const T pDt_yDt = pDt * yDt; - const T rDt_pDt = rDt * pDt; - const Vector3 remainder(tanp * rDt_pDt + one_over_cp * pDt_yDt, - -cp * rDt * yDt, - tanp * pDt_yDt + one_over_cp * rDt_pDt); - - // Combine terms that contains alpha with remainder terms. - // TODO(Mitiguy) M * alpha_AD_D can be calculated faster since the first - // column of M is (1, 0, 0). - return M * alpha_AD_D + remainder; - } - - private: - // Throws an exception if rpy is not a valid %RollPitchYaw. - // @param[in] rpy an allegedly valid rotation matrix. - // @note If the underlying scalar type T is non-numeric (symbolic), no - // validity check is made and no assertion is thrown. - template - static typename std::enable_if_t::is_bool> - ThrowIfNotValid(const Vector3& rpy) { - if (!RollPitchYaw::IsValid(rpy)) { - throw std::logic_error( - "Error: One (or more) of the roll-pitch-yaw angles is infinity or NaN."); - } - } - - template - static typename std::enable_if_t::is_bool> - ThrowIfNotValid(const Vector3&) {} - - // Throws an exception with a message that the pitch-angle `p` violates the - // internally-defined gimbal-lock tolerance, which occurs when `cos(p) ≈ 0`, - // which means `p ≈ (n*π + π/2)` where `n = 0, ±1, ±2, ...`. - // @param[in] function_name name of the calling function/method. - // @param[in] file_name name of the file with the calling function/method. - // @param[in] line_number the line number in file_name that made the call. - // @param[in] pitch_angle pitch angle `p` (in radians). - // @throws std::exception with a message that `p` is too near gimbal-lock. - static void ThrowPitchAngleViolatesGimbalLockTolerance( - const char* function_name, const char* file_name, const int line_number, - const T& pitch_angle); - - // Uses a quaternion and its associated rotation matrix `R` to accurately - // and efficiently set the roll-pitch-yaw angles (SpaceXYZ Euler angles) - // that underlie `this` @RollPitchYaw, even when the pitch angle p is very - // near a singularity (e.g., when p is within 1E-6 of π/2 or -π/2). - // After calling this method, the underlying roll-pitch-yaw `[r, p, y]` has - // range `-π <= r <= π`, `-π/2 <= p <= π/2`, `-π <= y <= π`. - // @param[in] quaternion unit quaternion with elements `[e0, e1, e2, e3]`. - // @param[in] R The %RotationMatrix corresponding to `quaternion`. - // @throws std::exception in debug builds if rpy fails IsValid(rpy). - // @note Aborts in debug builds if `quaternion` does not correspond to `R`. - void SetFromQuaternionAndRotationMatrix( - const Eigen::Quaternion& quaternion, const RotationMatrix& R); - - // For the %RotationMatrix `R` generated by `this` %RollPitchYaw, this method - // calculates the partial derivatives of `R` with respect to roll, pitch, yaw. - // @param[out] R_r ∂R/∂r Partial derivative of `R` with respect to roll `r`. - // @param[out] R_p ∂R/∂p Partial derivative of `R` with respect to pitch `p`. - // @param[out] R_y ∂R/∂y Partial derivative of `R` with respect to yaw `y`. - void CalcRotationMatrixDrDpDy(Matrix3* R_r, Matrix3* R_p, - Matrix3* R_y) const { - MALIPUT_DRAKE_ASSERT(R_r != nullptr && R_p != nullptr && R_y != nullptr); - const T& r = roll_angle(); - const T& p = pitch_angle(); - const T& y = yaw_angle(); - using std::sin; - using std::cos; - const T c0 = cos(r), c1 = cos(p), c2 = cos(y); - const T s0 = sin(r), s1 = sin(p), s2 = sin(y); - const T c2_s1 = c2 * s1, s2_s1 = s2 * s1, s2_s0 = s2 * s0, s2_c0 = s2 * c0; - const T c2_c1 = c2 * c1, s2_c1 = s2 * c1, c2_s0 = c2 * s0, c2_c0 = c2 * c0; - // clang-format on - *R_r << 0, c2_s1 * c0 + s2_s0, -c2_s1 * s0 + s2_c0, - 0, s2_s1 * c0 - c2_s0, -s2_s1 * s0 - c2_c0, - 0, c1 * c0, -c1 * s0; - - *R_p << -c2_s1, c2_c1 * s0, c2_c1 * c0, - -s2_s1, s2_c1 * s0, s2_c1 * c0, - -c1, -s1 * s0, -s1 * c0; - - *R_y << -s2_c1, -s2_s1 * s0 - c2_c0, -s2_s1 * c0 + c2_s0, - c2_c1, c2_s1 * s0 - s2_c0, c2_s1 * c0 + s2_s0, - 0, 0, 0; - // clang-format off - } - - // For `this` %RollPitchYaw with roll-pitch-yaw angles `[r; p; y]` which - // relate the orientation of two generic frames A and D, returns the 3x3 - // coefficient matrix M that contains the partial derivatives of `w_AD_A` - // (D's angular velocity in A, expressed in A) with respect to ṙ, ṗ, ẏ. - // In other words, `w_AD_A = M * rpyDt` where `rpyDt` is `[ṙ; ṗ; ẏ]`. - const Matrix3 CalcMatrixRelatingAngularVelocityInParentToRpyDt() const { - using std::cos; - using std::sin; - const T& p = pitch_angle(); - const T& y = yaw_angle(); - const T sp = sin(p), cp = cos(p); - const T sy = sin(y), cy = cos(y); - Matrix3 M; - // clang-format on - M << cp * cy, -sy, T(0), - cp * sy, cy, T(0), - -sp, T(0), T(1); - // clang-format off - return M; - } - - // Returns the time-derivative of the 3x3 matrix returned by the `this` - // %RollPitchYaw method MatrixRelatingAngularVelocityAToRpyDt(). - // @param[in] rpyDt Time-derivative of `[r; p; y]`, i.e., `[ṙ; ṗ; ẏ]`. - // @see MatrixRelatingAngularVelocityAToRpyDt() - const Matrix3 CalcDtMatrixRelatingAngularVelocityInParentToRpyDt( - const Vector3& rpyDt) const { - using std::cos; - using std::sin; - const T& p = pitch_angle(); - const T& y = yaw_angle(); - const T sp = sin(p), cp = cos(p); - const T sy = sin(y), cy = cos(y); - const T pDt = rpyDt(1); - const T yDt = rpyDt(2); - const T sp_pDt = sp * pDt; - const T cp_yDt = cp * yDt; - Matrix3 M; - // clang-format on - M << -cy * sp_pDt - sy * cp_yDt, -cy * yDt, T(0), - -sy * sp_pDt + cy * cp_yDt, -sy * yDt, T(0), - -cp * pDt, T(0), T(0); - // clang-format off - return M; - } - - // For `this` %RollPitchYaw with roll-pitch-yaw angles `[r; p; y]` which - // relate the orientation of two generic frames A and D, returns the 3x3 - // coefficient matrix M that contains the partial derivatives of `w_AD_D` - // (D's angular velocity in A, expressed in D) with respect to ṙ, ṗ, ẏ. - // In other words, `w_AD_D = M * rpyDt` where `rpyDt` is `[ṙ; ṗ; ẏ]`. - const Matrix3 CalcMatrixRelatingAngularVelocityInChildToRpyDt() const { - using std::cos; - using std::sin; - const T& r = roll_angle(); - const T& p = pitch_angle(); - const T sr = sin(r), cr = cos(r); - const T sp = sin(p), cp = cos(p); - Matrix3 M; - // clang-format on - M << T(1), T(0), -sp, - T(0), cr, sr * cp, - T(0), -sr, cr * cp; - // clang-format off - return M; - } - - // For `this` %RollPitchYaw with roll-pitch-yaw angles `[r; p; y]` which - // relate the orientation of two generic frames A and D, returns the 3x3 - // matrix M that contains the partial derivatives of [ṙ, ṗ, ẏ] with respect - // to `[wx; wy; wz]ₐ` (which is w_AD_A expressed in A). - // In other words, `rpyDt = M * w_AD_A`. - // @param[in] function_name name of the calling function/method. - // @param[in] file_name name of the file with the calling function/method. - // @param[in] line_number the line number in file_name that made the call. - // @throws std::exception if `cos(p) ≈ 0` (`p` is near gimbal-lock). - // @note This method has a divide-by-zero error (singularity) when the cosine - // of the pitch angle `p` is zero [i.e., `cos(p) = 0`]. This problem (called - // "gimbal lock") occurs when `p = n π + π / 2`, where n is any integer. - // There are associated precision problems (inaccuracies) in the neighborhood - // of these pitch angles, i.e., when `cos(p) ≈ 0`. - // @note This utility method typically gets called from a user-relevant API - // so it provides the ability to detect gimbal-lock and throws an error - // message that includes information from the calling function (rather than - // less useful information from within this method itself). - const Matrix3 CalcMatrixRelatingRpyDtToAngularVelocityInParent( - const char* function_name, const char* file_name, int line_number) const { - using std::cos; - using std::sin; - const T& p = pitch_angle(); - const T& y = yaw_angle(); - const T sp = sin(p), cp = cos(p); - // TODO(Mitiguy) Improve accuracy when `cos(p) ≈ 0`. - if (DoesCosPitchAngleViolateGimbalLockTolerance(cp)) { - ThrowPitchAngleViolatesGimbalLockTolerance(function_name, file_name, - line_number, p); - } - const T one_over_cp = T(1)/cp; - const T sy = sin(y), cy = cos(y); - const T cy_over_cp = cy * one_over_cp; - const T sy_over_cp = sy * one_over_cp; - Matrix3 M; - // clang-format on - M << cy_over_cp, sy_over_cp, T(0), - -sy, cy, T(0), - cy_over_cp * sp, sy_over_cp * sp, T(1); - // clang-format off - return M; - } - - - // Sets `this` %RollPitchYaw from a Vector3. - // @param[in] rpy allegedly valid roll-pitch-yaw angles. - // @throws std::exception in debug builds if rpy fails IsValid(rpy). - RollPitchYaw& SetOrThrowIfNotValidInDebugBuild(const Vector3& rpy) { - MALIPUT_DRAKE_ASSERT_VOID(ThrowIfNotValid(rpy)); - roll_pitch_yaw_ = rpy; - return *this; - } - - // Stores the underlying roll-pitch-yaw angles. - // There is no default initialization needed. - Vector3 roll_pitch_yaw_; - - // Internally-defined value for the allowable proximity of the cosine of the - // pitch-angle `p` to gimbal-lock [the proximity of `cos(p)` to 0]. - // @note For small values (<= 0.1), this value approximates the allowable - // proximity of the pitch-angle (in radians) to gimbal-lock. Example: A value - // of 0.01 corresponds to `p` within ≈ 0.01 radians (≈ 0.57°) of gimbal-lock, - // i.e., `p` is within 0.01 radians of `(n*π + π/2)`, `n = 0, ±1, ±2, ...`. - // A value 0.008 corresponds to `p` ≈ 0.008 radians (≈ 0.46°) of gimbal-lock. - // @note The conversion from angular velocity to rpyDt (the time-derivative of - // %RollPitchYaw) has a calculation that divides by `cos(p)` (the cosine of - // the pitch angle). This results in values of rpyDt that scale with angular - // velocity multiplied by `1/cos(p)`, which can cause problems with numerical - // precision and numerical integration. - // @note There is a numerical test (in roll_pitch_yaw_test.cc) that converts - // an angular velocity w(1, 1, 1) to rpyDt (time-derivative of roll-pitch-yaw) - // and then back to angular velocity. This test revealed an imprecision in - // the back-and-forth calculation near gimbal-lock by calculating max_error - // (how much the angular velocity changed in the back-and-forth calculation). - // The table below shows various values of kGimbalLockToleranceCosPitchAngle, - // it associated proximity to gimbal-lock (in degrees) and the associated - // max_error (in terms of machine kEpsilon = 1/2^52 ≈ 2.22E-16). - // --------------------------------------------------------------------------- - // kGimbalLockToleranceCosPitchAngle | max_error - // --------------------------------------------------------------------------- - // 0.001 ≈ 0.06° | (2^10 = 1024) * kEpsilon ≈ 2.274E-13 - // 0.002 ≈ 0.11° | (2^9 = 512) * kEpsilon ≈ 1.137E-13 - // 0.004 ≈ 0.23° | (2^8 = 256) * kEpsilon ≈ 5.684E-14 - // 0.008 ≈ 0.46° | (2^7 = 128) * kEpsilon ≈ 2.842E-14 - // 0.016 ≈ 0.92° | (2^6 = 64) * kEpsilon ≈ 1.421E-14 - // 0.032 ≈ 1.83° | (2^5 = 32) * kEpsilon ≈ 7.105E-15 - // --------------------------------------------------------------------------- - // Hence if kGimbalLockToleranceCosPitchAngle = 0.008 and the pitch angle is - // in the proximity of ≈ 0.46° of gimbal lock, there may be inaccuracies in - // 7 of the 52 bits in max_error's mantissa, which we deem acceptable. - static constexpr double kGimbalLockToleranceCosPitchAngle = 0.008; -}; - -/// Stream insertion operator to write an instance of RollPitchYaw into a -/// `std::ostream`. Especially useful for debugging. -/// @relates RollPitchYaw. -template -inline std::ostream& operator<<(std::ostream& out, const RollPitchYaw& rpy) { - const T& roll = rpy.roll_angle(); - const T& pitch = rpy.pitch_angle(); - const T& yaw = rpy.yaw_angle(); - if constexpr (scalar_predicate::is_bool) { - out << fmt::format("rpy = {} {} {}", roll, pitch, yaw); - } else { - out << "rpy = " << roll << " " << pitch << " " << yaw; - } - return out; -} - -/// Abbreviation (alias/typedef) for a RollPitchYaw double scalar type. -/// @relates RollPitchYaw -using RollPitchYawd = RollPitchYaw; - -} // namespace math -} // namespace maliput::drake - -DRAKE_DECLARE_CLASS_TEMPLATE_INSTANTIATIONS_ON_DEFAULT_SCALARS( - class ::maliput::drake::math::RollPitchYaw) diff --git a/include/maliput/drake/math/rotation_conversion_gradient.h b/include/maliput/drake/math/rotation_conversion_gradient.h deleted file mode 100644 index da88917..0000000 --- a/include/maliput/drake/math/rotation_conversion_gradient.h +++ /dev/null @@ -1,230 +0,0 @@ -#pragma once - -#include - -#include "maliput/drake/common/constants.h" -#include "maliput/drake/math/gradient.h" -#include "maliput/drake/math/gradient_util.h" -#include "maliput/drake/math/normalize_vector.h" - -namespace maliput::drake { -namespace math { -/** - * Computes the gradient of the function that converts a unit length quaternion - * to a rotation matrix. - * @param quaternion A unit length quaternion [w;x;y;z] - * @return The gradient - */ -template -typename maliput::drake::math::Gradient, - maliput::drake::kQuaternionSize>::type -dquat2rotmat(const Eigen::MatrixBase& quaternion) { - EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(Eigen::MatrixBase, - maliput::drake::kQuaternionSize); - - typename maliput::drake::math::Gradient, - maliput::drake::kQuaternionSize>::type ret; - typename Eigen::MatrixBase::PlainObject qtilde; - typename maliput::drake::math::Gradient::type dqtilde; - maliput::drake::math::NormalizeVector(quaternion, qtilde, &dqtilde); - - typedef typename Derived::Scalar Scalar; - Scalar w = qtilde(0); - Scalar x = qtilde(1); - Scalar y = qtilde(2); - Scalar z = qtilde(3); - - ret << w, x, -y, -z, z, y, x, w, -y, z, -w, x, -z, y, x, -w, w, -x, y, -z, x, - w, z, y, y, z, w, x, -x, -w, z, y, w, -x, -y, z; - ret *= 2.0; - ret *= dqtilde; - return ret; -} - -/** - * Computes the gradient of the function that converts a rotation matrix to - * body-fixed z-y'-x'' Euler angles. - * @param R A 3 x 3 rotation matrix - * @param dR A 9 x N matrix, dR(i,j) is the gradient of R(i) w.r.t x(j) - * @return The gradient G. G is a 3 x N matrix. - * G(0,j) is the gradient of roll w.r.t x(j) - * G(1,j) is the gradient of pitch w.r.t x(j) - * G(2,j) is the gradient of yaw w.r.t x(j) - */ -template -typename maliput::drake::math::Gradient< - Eigen::Matrix, - DerivedDR::ColsAtCompileTime>::type -drotmat2rpy(const Eigen::MatrixBase& R, - const Eigen::MatrixBase& dR) { - EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_SIZE(Eigen::MatrixBase, - maliput::drake::kSpaceDimension, - maliput::drake::kSpaceDimension); - EIGEN_STATIC_ASSERT( - Eigen::MatrixBase::RowsAtCompileTime == maliput::drake::kRotmatSize, - THIS_METHOD_IS_ONLY_FOR_MATRICES_OF_A_SPECIFIC_SIZE); - - typename DerivedDR::Index nq = dR.cols(); - typedef typename DerivedR::Scalar Scalar; - typedef - typename maliput::drake::math::Gradient, - DerivedDR::ColsAtCompileTime>::type - ReturnType; - ReturnType drpy(maliput::drake::kRpySize, nq); - - auto dR11_dq = - getSubMatrixGradient(dR, 0, 0, R.rows()); - auto dR21_dq = - getSubMatrixGradient(dR, 1, 0, R.rows()); - auto dR31_dq = - getSubMatrixGradient(dR, 2, 0, R.rows()); - auto dR32_dq = - getSubMatrixGradient(dR, 2, 1, R.rows()); - auto dR33_dq = - getSubMatrixGradient(dR, 2, 2, R.rows()); - - Scalar sqterm = R(2, 1) * R(2, 1) + R(2, 2) * R(2, 2); - - // droll_dq - drpy.row(0) = (R(2, 2) * dR32_dq - R(2, 1) * dR33_dq) / sqterm; - - // dpitch_dq - using namespace std; // NOLINT(build/namespaces) - Scalar sqrt_sqterm = sqrt(sqterm); - drpy.row(1) = - (-sqrt_sqterm * dR31_dq + - R(2, 0) / sqrt_sqterm * (R(2, 1) * dR32_dq + R(2, 2) * dR33_dq)) / - (R(2, 0) * R(2, 0) + R(2, 1) * R(2, 1) + R(2, 2) * R(2, 2)); - - // dyaw_dq - sqterm = R(0, 0) * R(0, 0) + R(1, 0) * R(1, 0); - drpy.row(2) = (R(0, 0) * dR21_dq - R(1, 0) * dR11_dq) / sqterm; - return drpy; -} - -/** - * Computes the gradient of the function that converts rotation matrix to - * quaternion. - * @param R A 3 x 3 rotation matrix - * @param dR A 9 x N matrix, dR(i,j) is the gradient of R(i) w.r.t x_var(j) - * @return The gradient G. G is a 4 x N matrix - * G(0,j) is the gradient of w w.r.t x_var(j) - * G(1,j) is the gradient of x w.r.t x_var(j) - * G(2,j) is the gradient of y w.r.t x_var(j) - * G(3,j) is the gradient of z w.r.t x_var(j) - */ -template -typename maliput::drake::math::Gradient< - Eigen::Matrix, - DerivedDR::ColsAtCompileTime>::type -drotmat2quat(const Eigen::MatrixBase& R, - const Eigen::MatrixBase& dR) { - EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_SIZE(Eigen::MatrixBase, - maliput::drake::kSpaceDimension, - maliput::drake::kSpaceDimension); - EIGEN_STATIC_ASSERT( - Eigen::MatrixBase::RowsAtCompileTime == maliput::drake::kRotmatSize, - THIS_METHOD_IS_ONLY_FOR_MATRICES_OF_A_SPECIFIC_SIZE); - - typedef typename DerivedR::Scalar Scalar; - typedef typename maliput::drake::math::Gradient< - Eigen::Matrix, - DerivedDR::ColsAtCompileTime>::type ReturnType; - typename DerivedDR::Index nq = dR.cols(); - - auto dR11_dq = - getSubMatrixGradient(dR, 0, 0, R.rows()); - auto dR12_dq = - getSubMatrixGradient(dR, 0, 1, R.rows()); - auto dR13_dq = - getSubMatrixGradient(dR, 0, 2, R.rows()); - auto dR21_dq = - getSubMatrixGradient(dR, 1, 0, R.rows()); - auto dR22_dq = - getSubMatrixGradient(dR, 1, 1, R.rows()); - auto dR23_dq = - getSubMatrixGradient(dR, 1, 2, R.rows()); - auto dR31_dq = - getSubMatrixGradient(dR, 2, 0, R.rows()); - auto dR32_dq = - getSubMatrixGradient(dR, 2, 1, R.rows()); - auto dR33_dq = - getSubMatrixGradient(dR, 2, 2, R.rows()); - - Eigen::Matrix A; - A.row(0) << 1.0, 1.0, 1.0; - A.row(1) << 1.0, -1.0, -1.0; - A.row(2) << -1.0, 1.0, -1.0; - A.row(3) << -1.0, -1.0, 1.0; - Eigen::Matrix B = A * R.diagonal(); - typename Eigen::Matrix::Index ind, max_col; - Scalar val = B.maxCoeff(&ind, &max_col); - - ReturnType dq(maliput::drake::kQuaternionSize, nq); - using namespace std; // NOLINT(build/namespaces) - switch (ind) { - case 0: { - // val = trace(M) - auto dvaldq = dR11_dq + dR22_dq + dR33_dq; - auto dwdq = dvaldq / (4.0 * sqrt(1.0 + val)); - auto w = sqrt(1.0 + val) / 2.0; - auto wsquare4 = 4.0 * w * w; - dq.row(0) = dwdq; - dq.row(1) = - ((dR32_dq - dR23_dq) * w - (R(2, 1) - R(1, 2)) * dwdq) / wsquare4; - dq.row(2) = - ((dR13_dq - dR31_dq) * w - (R(0, 2) - R(2, 0)) * dwdq) / wsquare4; - dq.row(3) = - ((dR21_dq - dR12_dq) * w - (R(1, 0) - R(0, 1)) * dwdq) / wsquare4; - break; - } - case 1: { - // val = M(1, 1) - M(2, 2) - M(3, 3) - auto dvaldq = dR11_dq - dR22_dq - dR33_dq; - auto s = 2.0 * sqrt(1.0 + val); - auto ssquare = s * s; - auto dsdq = dvaldq / sqrt(1.0 + val); - dq.row(0) = - ((dR32_dq - dR23_dq) * s - (R(2, 1) - R(1, 2)) * dsdq) / ssquare; - dq.row(1) = .25 * dsdq; - dq.row(2) = - ((dR12_dq + dR21_dq) * s - (R(0, 1) + R(1, 0)) * dsdq) / ssquare; - dq.row(3) = - ((dR13_dq + dR31_dq) * s - (R(0, 2) + R(2, 0)) * dsdq) / ssquare; - break; - } - case 2: { - // val = M(2, 2) - M(1, 1) - M(3, 3) - auto dvaldq = -dR11_dq + dR22_dq - dR33_dq; - auto s = 2.0 * (sqrt(1.0 + val)); - auto ssquare = s * s; - auto dsdq = dvaldq / sqrt(1.0 + val); - dq.row(0) = - ((dR13_dq - dR31_dq) * s - (R(0, 2) - R(2, 0)) * dsdq) / ssquare; - dq.row(1) = - ((dR12_dq + dR21_dq) * s - (R(0, 1) + R(1, 0)) * dsdq) / ssquare; - dq.row(2) = .25 * dsdq; - dq.row(3) = - ((dR23_dq + dR32_dq) * s - (R(1, 2) + R(2, 1)) * dsdq) / ssquare; - break; - } - default: { - // val = M(3, 3) - M(2, 2) - M(1, 1) - auto dvaldq = -dR11_dq - dR22_dq + dR33_dq; - auto s = 2.0 * (sqrt(1.0 + val)); - auto ssquare = s * s; - auto dsdq = dvaldq / sqrt(1.0 + val); - dq.row(0) = - ((dR21_dq - dR12_dq) * s - (R(1, 0) - R(0, 1)) * dsdq) / ssquare; - dq.row(1) = - ((dR13_dq + dR31_dq) * s - (R(0, 2) + R(2, 0)) * dsdq) / ssquare; - dq.row(2) = - ((dR23_dq + dR32_dq) * s - (R(1, 2) + R(2, 1)) * dsdq) / ssquare; - dq.row(3) = .25 * dsdq; - break; - } - } - return dq; -} -} // namespace math -} // namespace maliput::drake diff --git a/include/maliput/drake/math/rotation_matrix.h b/include/maliput/drake/math/rotation_matrix.h deleted file mode 100644 index 7a3fe34..0000000 --- a/include/maliput/drake/math/rotation_matrix.h +++ /dev/null @@ -1,1099 +0,0 @@ -#pragma once - -#include -#include -#include -#include -#include - -#include - -#include "maliput/drake/common/default_scalars.h" -#include "maliput/drake/common/drake_assert.h" -#include "maliput/drake/common/drake_bool.h" -#include "maliput/drake/common/drake_copyable.h" -#include "maliput/drake/common/drake_throw.h" -#include "maliput/drake/common/eigen_types.h" -#include "maliput/drake/common/never_destroyed.h" -#include "maliput/drake/common/symbolic.h" -#include "maliput/drake/math/fast_pose_composition_functions.h" -#include "maliput/drake/math/roll_pitch_yaw.h" - -namespace maliput::drake { -namespace math { - -namespace internal { -// This is used to select a non-initializing constructor for use by -// RigidTransform. -struct DoNotInitializeMemberFields {}; -} - -/// This class represents a 3x3 rotation matrix between two arbitrary frames -/// A and B and helps ensure users create valid rotation matrices. This class -/// relates right-handed orthogonal unit vectors Ax, Ay, Az fixed in frame A -/// to right-handed orthogonal unit vectors Bx, By, Bz fixed in frame B. -/// The monogram notation for the rotation matrix relating A to B is `R_AB`. -/// An example that gives context to this rotation matrix is `v_A = R_AB * v_B`, -/// where `v_B` denotes an arbitrary vector v expressed in terms of Bx, By, Bz -/// and `v_A` denotes vector v expressed in terms of Ax, Ay, Az. -/// See @ref multibody_quantities for monogram notation for dynamics. -/// See @ref orientation_discussion "a discussion on rotation matrices". -/// -/// @note This class does not store the frames associated with a rotation matrix -/// nor does it enforce strict proper usage of this class with vectors. -/// -/// @note When assertions are enabled, several methods in this class -/// do a validity check and throw an exception (std::exception) if the -/// rotation matrix is invalid. When assertions are disabled, -/// many of these validity checks are skipped (which helps improve speed). -/// In addition, these validity tests are only performed for scalar types for -/// which maliput::drake::scalar_predicate::is_bool is `true`. For instance, validity -/// checks are not performed when T is symbolic::Expression. -/// -/// @authors Paul Mitiguy (2018) Original author. -/// @authors Drake team (see https://drake.mit.edu/credits). -/// -/// @tparam_default_scalar -template -class RotationMatrix { - public: - DRAKE_DEFAULT_COPY_AND_MOVE_AND_ASSIGN(RotationMatrix) - - /// Constructs a 3x3 identity %RotationMatrix -- which corresponds to - /// aligning two frames (so that unit vectors Ax = Bx, Ay = By, Az = Bz). - RotationMatrix() : R_AB_(Matrix3::Identity()) {} - - /// Constructs a %RotationMatrix from a Matrix3. - /// @param[in] R an allegedly valid rotation matrix. - /// @throws std::exception in debug builds if R fails IsValid(R). - explicit RotationMatrix(const Matrix3& R) { set(R); } - - /// Constructs a %RotationMatrix from an Eigen::Quaternion. - /// @param[in] quaternion a non-zero, finite quaternion which may or may not - /// have unit length [i.e., `quaternion.norm()` does not have to be 1]. - /// @throws std::exception in debug builds if the rotation matrix - /// R that is built from `quaternion` fails IsValid(R). For example, an - /// exception is thrown if `quaternion` is zero or contains a NaN or infinity. - /// @note This method has the effect of normalizing its `quaternion` argument, - /// without the inefficiency of the square-root associated with normalization. - explicit RotationMatrix(const Eigen::Quaternion& quaternion) { - // TODO(mitiguy) Although this method is fairly efficient, consider adding - // an optional second argument if `quaternion` is known to be normalized - // apriori or for some reason the calling site does not want `quaternion` - // normalized. - // Cost for various way to create a rotation matrix from a quaternion. - // Eigen quaternion.toRotationMatrix() = 12 multiplies, 12 adds. - // Drake QuaternionToRotationMatrix() = 12 multiplies, 12 adds. - // Extra cost for two_over_norm_squared = 4 multiplies, 3 adds, 1 divide. - // Extra cost if normalized = 4 multiplies, 3 adds, 1 sqrt, 1 divide. - const T two_over_norm_squared = T(2) / quaternion.squaredNorm(); - set(QuaternionToRotationMatrix(quaternion, two_over_norm_squared)); - } - - // @internal In general, the %RotationMatrix constructed by passing a non-unit - // `lambda` to this method is different than the %RotationMatrix produced by - // converting `lambda` to an un-normalized quaternion and calling the - // %RotationMatrix constructor (above) with that un-normalized quaternion. - /// Constructs a %RotationMatrix from an Eigen::AngleAxis. - /// @param[in] theta_lambda an Eigen::AngleAxis whose associated axis (vector - /// direction herein called `lambda`) is non-zero and finite, but which may or - /// may not have unit length [i.e., `lambda.norm()` does not have to be 1]. - /// @throws std::exception in debug builds if the rotation matrix - /// R that is built from `theta_lambda` fails IsValid(R). For example, an - /// exception is thrown if `lambda` is zero or contains a NaN or infinity. - explicit RotationMatrix(const Eigen::AngleAxis& theta_lambda) { - // TODO(mitiguy) Consider adding an optional second argument if `lambda` is - // known to be normalized apriori or calling site does not want - // normalization. - const Vector3& lambda = theta_lambda.axis(); - const T norm = lambda.norm(); - const T& theta = theta_lambda.angle(); - set(Eigen::AngleAxis(theta, lambda / norm).toRotationMatrix()); - } - - /// Constructs a %RotationMatrix from an %RollPitchYaw. In other words, - /// makes the %RotationMatrix for a Space-fixed (extrinsic) X-Y-Z rotation by - /// "roll-pitch-yaw" angles `[r, p, y]`, which is equivalent to a Body-fixed - /// (intrinsic) Z-Y-X rotation by "yaw-pitch-roll" angles `[y, p, r]`. - /// @param[in] rpy radian measures of three angles [roll, pitch, yaw]. - /// @param[in] rpy a %RollPitchYaw which is a Space-fixed (extrinsic) X-Y-Z - /// rotation with "roll-pitch-yaw" angles `[r, p, y]` or equivalently a Body- - /// fixed (intrinsic) Z-Y-X rotation with "yaw-pitch-roll" angles `[y, p, r]`. - /// @note Denoting roll `r`, pitch `p`, yaw `y`, this method returns a - /// rotation matrix `R_AD` equal to the matrix multiplication shown below. - /// ``` - /// ⎡cos(y) -sin(y) 0⎤ ⎡ cos(p) 0 sin(p)⎤ ⎡1 0 0 ⎤ - /// R_AD = ⎢sin(y) cos(y) 0⎥ * ⎢ 0 1 0 ⎥ * ⎢0 cos(r) -sin(r)⎥ - /// ⎣ 0 0 1⎦ ⎣-sin(p) 0 cos(p)⎦ ⎣0 sin(r) cos(r)⎦ - /// = R_AB * R_BC * R_CD - /// ``` - /// @note In this discussion, A is the Space frame and D is the Body frame. - /// One way to visualize this rotation sequence is by introducing intermediate - /// frames B and C (useful constructs to understand this rotation sequence). - /// Initially, the frames are aligned so `Di = Ci = Bi = Ai (i = x, y, z)`. - /// Then D is subjected to successive right-handed rotations relative to A. - /// @li 1st rotation R_CD: %Frame D rotates relative to frames C, B, A by a - /// roll angle `r` about `Dx = Cx`. Note: D and C are no longer aligned. - /// @li 2nd rotation R_BC: Frames D, C (collectively -- as if welded together) - /// rotate relative to frame B, A by a pitch angle `p` about `Cy = By`. - /// Note: C and B are no longer aligned. - /// @li 3rd rotation R_AB: Frames D, C, B (collectively -- as if welded) - /// rotate relative to frame A by a roll angle `y` about `Bz = Az`. - /// Note: B and A are no longer aligned. - /// @note This method constructs a RotationMatrix from a RollPitchYaw. - /// Vice-versa, there are high-accuracy RollPitchYaw constructor/methods that - /// form a RollPitchYaw from a rotation matrix. - explicit RotationMatrix(const RollPitchYaw& rpy) { - // TODO(@mitiguy) Add publicly viewable documentation on how Sherm and - // Goldstein like to visualize/conceptualize rotation sequences. - const T& r = rpy.roll_angle(); - const T& p = rpy.pitch_angle(); - const T& y = rpy.yaw_angle(); - using std::sin; - using std::cos; - const T c0 = cos(r), c1 = cos(p), c2 = cos(y); - const T s0 = sin(r), s1 = sin(p), s2 = sin(y); - const T c2_s1 = c2 * s1, s2_s1 = s2 * s1; - const T Rxx = c2 * c1; - const T Rxy = c2_s1 * s0 - s2 * c0; - const T Rxz = c2_s1 * c0 + s2 * s0; - const T Ryx = s2 * c1; - const T Ryy = s2_s1 * s0 + c2 * c0; - const T Ryz = s2_s1 * c0 - c2 * s0; - const T Rzx = -s1; - const T Rzy = c1 * s0; - const T Rzz = c1 * c0; - SetFromOrthonormalRows(Vector3(Rxx, Rxy, Rxz), - Vector3(Ryx, Ryy, Ryz), - Vector3(Rzx, Rzy, Rzz)); - } - - /// (Advanced) Makes the %RotationMatrix `R_AB` from right-handed orthogonal - /// unit vectors `Bx`, `By`, `Bz` so the columns of `R_AB` are `[Bx, By, Bz]`. - /// @param[in] Bx first unit vector in right-handed orthogonal set. - /// @param[in] By second unit vector in right-handed orthogonal set. - /// @param[in] Bz third unit vector in right-handed orthogonal set. - /// @throws std::exception in debug builds if `R_AB` fails IsValid(R_AB). - /// @note In release builds, the caller can subsequently test if `R_AB` is, - /// in fact, a valid %RotationMatrix by calling `R_AB.IsValid()`. - /// @note The rotation matrix `R_AB` relates two sets of right-handed - /// orthogonal unit vectors, namely Ax, Ay, Az and Bx, By, Bz. - /// The rows of `R_AB` are Ax, Ay, Az expressed in frame B (i.e.,`Ax_B`, - /// `Ay_B`, `Az_B`). The columns of `R_AB` are Bx, By, Bz expressed in - /// frame A (i.e., `Bx_A`, `By_A`, `Bz_A`). - static RotationMatrix MakeFromOrthonormalColumns( - const Vector3& Bx, const Vector3& By, const Vector3& Bz) { - RotationMatrix R(internal::DoNotInitializeMemberFields{}); - R.SetFromOrthonormalColumns(Bx, By, Bz); - return R; - } - - /// (Advanced) Makes the %RotationMatrix `R_AB` from right-handed orthogonal - /// unit vectors `Ax`, `Ay`, `Az` so the rows of `R_AB` are `[Ax, Ay, Az]`. - /// @param[in] Ax first unit vector in right-handed orthogonal set. - /// @param[in] Ay second unit vector in right-handed orthogonal set. - /// @param[in] Az third unit vector in right-handed orthogonal set. - /// @throws std::exception in debug builds if `R_AB` fails IsValid(R_AB). - /// @note In release builds, the caller can subsequently test if `R_AB` is, - /// in fact, a valid %RotationMatrix by calling `R_AB.IsValid()`. - /// @note The rotation matrix `R_AB` relates two sets of right-handed - /// orthogonal unit vectors, namely Ax, Ay, Az and Bx, By, Bz. - /// The rows of `R_AB` are Ax, Ay, Az expressed in frame B (i.e.,`Ax_B`, - /// `Ay_B`, `Az_B`). The columns of `R_AB` are Bx, By, Bz expressed in - /// frame A (i.e., `Bx_A`, `By_A`, `Bz_A`). - static RotationMatrix MakeFromOrthonormalRows( - const Vector3& Ax, const Vector3& Ay, const Vector3& Az) { - RotationMatrix R(internal::DoNotInitializeMemberFields{}); - R.SetFromOrthonormalRows(Ax, Ay, Az); - return R; - } - - /// Makes the %RotationMatrix `R_AB` associated with rotating a frame B - /// relative to a frame A by an angle `theta` about unit vector `Ax = Bx`. - /// @param[in] theta radian measure of rotation angle about Ax. - /// @note Orientation is same as Eigen::AngleAxis(theta, Vector3d::UnitX(). - /// @note `R_AB` relates two frames A and B having unit vectors Ax, Ay, Az and - /// Bx, By, Bz. Initially, `Bx = Ax`, `By = Ay`, `Bz = Az`, then B undergoes - /// a right-handed rotation relative to A by an angle `theta` about `Ax = Bx`. - /// ``` - /// ⎡ 1 0 0 ⎤ - /// R_AB = ⎢ 0 cos(theta) -sin(theta) ⎥ - /// ⎣ 0 sin(theta) cos(theta) ⎦ - /// ``` - static RotationMatrix MakeXRotation(const T& theta) { - Matrix3 R; - using std::sin; - using std::cos; - const T c = cos(theta), s = sin(theta); - // clang-format off - R << 1, 0, 0, - 0, c, -s, - 0, s, c; - // clang-format on - return RotationMatrix(R); - } - - /// Makes the %RotationMatrix `R_AB` associated with rotating a frame B - /// relative to a frame A by an angle `theta` about unit vector `Ay = By`. - /// @param[in] theta radian measure of rotation angle about Ay. - /// @note Orientation is same as Eigen::AngleAxis(theta, Vector3d::UnitY(). - /// @note `R_AB` relates two frames A and B having unit vectors Ax, Ay, Az and - /// Bx, By, Bz. Initially, `Bx = Ax`, `By = Ay`, `Bz = Az`, then B undergoes - /// a right-handed rotation relative to A by an angle `theta` about `Ay = By`. - /// ``` - /// ⎡ cos(theta) 0 sin(theta) ⎤ - /// R_AB = ⎢ 0 1 0 ⎥ - /// ⎣ -sin(theta) 0 cos(theta) ⎦ - /// ``` - static RotationMatrix MakeYRotation(const T& theta) { - Matrix3 R; - using std::sin; - using std::cos; - const T c = cos(theta), s = sin(theta); - // clang-format off - R << c, 0, s, - 0, 1, 0, - -s, 0, c; - // clang-format on - return RotationMatrix(R); - } - - /// Makes the %RotationMatrix `R_AB` associated with rotating a frame B - /// relative to a frame A by an angle `theta` about unit vector `Az = Bz`. - /// @param[in] theta radian measure of rotation angle about Az. - /// @note Orientation is same as Eigen::AngleAxis(theta, Vector3d::UnitZ(). - /// @note `R_AB` relates two frames A and B having unit vectors Ax, Ay, Az and - /// Bx, By, Bz. Initially, `Bx = Ax`, `By = Ay`, `Bz = Az`, then B undergoes - /// a right-handed rotation relative to A by an angle `theta` about `Az = Bz`. - /// ``` - /// ⎡ cos(theta) -sin(theta) 0 ⎤ - /// R_AB = ⎢ sin(theta) cos(theta) 0 ⎥ - /// ⎣ 0 0 1 ⎦ - /// ``` - static RotationMatrix MakeZRotation(const T& theta) { - Matrix3 R; - using std::sin; - using std::cos; - const T c = cos(theta), s = sin(theta); - // clang-format off - R << c, -s, 0, - s, c, 0, - 0, 0, 1; - // clang-format on - return RotationMatrix(R); - } - - /// Creates a 3D right-handed orthonormal basis B from a given vector b_A, - /// returned as a rotation matrix R_AB. It consists of orthogonal unit vectors - /// u_A, v_A, w_A where u_A is the normalized b_A in the axis_index column of - /// R_AB and v_A has one element which is zero. If an element of b_A is zero, - /// then one element of w_A is 1 and the other two elements are 0. - /// @param[in] b_A vector expressed in frame A that when normalized as - /// u_A = b_A.normalized() represents Bx, By, or Bz (depending on axis_index). - /// @param[in] axis_index The index ∈ {0, 1, 2} of the unit vector associated - /// with u_A, 0 means u_A is Bx, 1 means u_A is By, and 2 means u_A is Bz. - /// @pre axis_index is 0 or 1 or 2. - /// @throws std::exception if b_A cannot be made into a unit vector because - /// b_A contains a NaN or infinity or |b_A| < 1.0E-10. - /// @throws std::exception if the underlying type is symbolic (non-numeric). - /// @see MakeFromOneUnitVector() if b_A is known to already be unit length. - /// @retval R_AB the rotation matrix with properties as described above. - static RotationMatrix MakeFromOneVector( - const Vector3& b_A, int axis_index) { - const Vector3 u_A = NormalizeOrThrow(b_A, __func__); - return MakeFromOneUnitVector(u_A, axis_index); - } - - /// (Advanced) Creates a right-handed orthonormal basis B from a given - /// unit vector u_A, returned as a rotation matrix R_AB. - /// @param[in] u_A unit vector which is expressed in frame A and is either - /// Bx or By or Bz (depending on the value of axis_index). - /// @param[in] axis_index The index ∈ {0, 1, 2} of the unit vector associated - /// with u_A, 0 means u_A is Bx, 1 means u_A is By, and 2 means u_A is Bz. - /// @pre axis_index is 0 or 1 or 2. - /// @throws std::exception in Debug builds if u_A is not a unit vector, i.e., - /// |u_A| is not within a tolerance of 4ε ≈ 8.88E-16 to 1.0. - /// @throws std::exception if the underlying type is symbolic (non-numeric). - /// @note This method is designed for maximum performance and does not verify - /// u_A as a unit vector in Release builds. Consider MakeFromOneVector(). - /// @retval R_AB the rotation matrix whose properties are described in - /// MakeFromOneVector(). - static RotationMatrix MakeFromOneUnitVector(const Vector3& u_A, - int axis_index); - - /// Creates a %RotationMatrix templatized on a scalar type U from a - /// %RotationMatrix templatized on scalar type T. For example, - /// ``` - /// RotationMatrix source = RotationMatrix::Identity(); - /// RotationMatrix foo = source.cast(); - /// ``` - /// @tparam U Scalar type on which the returned %RotationMatrix is templated. - /// @note `RotationMatrix::cast()` creates a new - /// `RotationMatrix` from a `RotationMatrix` but only if - /// type `To` is constructible from type `From`. - /// This cast method works in accordance with Eigen's cast method for Eigen's - /// %Matrix3 that underlies this %RotationMatrix. For example, Eigen - /// currently allows cast from type double to AutoDiffXd, but not vice-versa. - template - RotationMatrix cast() const { - // TODO(Mitiguy) Make the RotationMatrix::cast() method more robust. It is - // currently limited by Eigen's cast() for the matrix underlying this class. - // Consider the following logic to improve casts (and address issue #11785). - // 1. If relevant, use Eigen's underlying cast method. - // 2. Strip derivative data when casting from `` to ``. - // 3. Call ExtractDoubleOrThrow() when casting from `` - // to ``. - // 4. The current RotationMatrix::cast() method incurs overhead due to its - // underlying call to a RotationMatrix constructor. Perhaps create - // specialized code to return a reference if casting to the same type, - // e.g., casting from `` to `' should be inexpensive. - const Matrix3 m = R_AB_.template cast(); - return RotationMatrix(m, true); - } - - /// Sets `this` %RotationMatrix from a Matrix3. - /// @param[in] R an allegedly valid rotation matrix. - /// @throws std::exception in debug builds if R fails IsValid(R). - void set(const Matrix3& R) { - MALIPUT_DRAKE_ASSERT_VOID(ThrowIfNotValid(R)); - SetUnchecked(R); - } - - /// Returns the 3x3 identity %RotationMatrix. - // @internal This method's name was chosen to mimic Eigen's Identity(). - static const RotationMatrix& Identity() { - static const never_destroyed> kIdentity; - return kIdentity.access(); - } - - /// Returns `R_BA = R_AB⁻¹`, the inverse (transpose) of this %RotationMatrix. - /// @note For a valid rotation matrix `R_BA = R_AB⁻¹ = R_ABᵀ`. - // @internal This method's name was chosen to mimic Eigen's inverse(). - RotationMatrix inverse() const { - return RotationMatrix(R_AB_.transpose()); - } - - /// Returns `R_BA = R_AB⁻¹`, the transpose of this %RotationMatrix. - /// @note For a valid rotation matrix `R_BA = R_AB⁻¹ = R_ABᵀ`. - // @internal This method's name was chosen to mimic Eigen's transpose(). - RotationMatrix transpose() const { - return RotationMatrix(R_AB_.transpose()); - } - - /// Returns the Matrix3 underlying a %RotationMatrix. - /// @see col(), row() - const Matrix3& matrix() const { return R_AB_; } - - /// Returns `this` rotation matrix's iᵗʰ row (i = 0, 1, 2). - /// For `this` rotation matrix R_AB (which relates right-handed - /// sets of orthogonal unit vectors Ax, Ay, Az to Bx, By, Bz), - /// - row(0) returns Ax_B (Ax expressed in terms of Bx, By, Bz). - /// - row(1) returns Ay_B (Ay expressed in terms of Bx, By, Bz). - /// - row(2) returns Az_B (Az expressed in terms of Bx, By, Bz). - /// @param[in] index requested row index (0 <= index <= 2). - /// @see col(), matrix() - /// @throws In Debug builds, asserts (0 <= index <= 2). - /// @note For efficiency and consistency with Eigen, this method returns - /// the same quantity returned by Eigen's row() operator. - /// The returned quantity can be assigned in various ways, e.g., as - /// `const auto& Az_B = row(2);` or `RowVector3 Az_B = row(2);` - const Eigen::Block, 1, 3, false> row(int index) const { - // The returned value from this method mimics Eigen's row() method which was - // found in Eigen/src/plugins/BlockMethods.h. The Eigen Matrix3 R_AB_ that - // underlies this class is a column major matrix. To return a row, - // InnerPanel = false is passed as the last template parameter above. - MALIPUT_DRAKE_ASSERT(0 <= index && index <= 2); - return R_AB_.row(index); - } - - /// Returns `this` rotation matrix's iᵗʰ column (i = 0, 1, 2). - /// For `this` rotation matrix R_AB (which relates right-handed - /// sets of orthogonal unit vectors Ax, Ay, Az to Bx, By, Bz), - /// - col(0) returns Bx_A (Bx expressed in terms of Ax, Ay, Az). - /// - col(1) returns By_A (By expressed in terms of Ax, Ay, Az). - /// - col(2) returns Bz_A (Bz expressed in terms of Ax, Ay, Az). - /// @param[in] index requested column index (0 <= index <= 2). - /// @see row(), matrix() - /// @throws In Debug builds, asserts (0 <= index <= 2). - /// @note For efficiency and consistency with Eigen, this method returns - /// the same quantity returned by Eigen's col() operator. - /// The returned quantity can be assigned in various ways, e.g., as - /// `const auto& Bz_A = col(2);` or `Vector3 Bz_A = col(2);` - const Eigen::Block, 3, 1, true> col(int index) const { - // The returned value from this method mimics Eigen's col() method which was - // found in Eigen/src/plugins/BlockMethods.h. The Eigen Matrix3 R_AB_ that - // underlies this class is a column major matrix. To return a column, - // InnerPanel = true is passed as the last template parameter above. - MALIPUT_DRAKE_ASSERT(0 <= index && index <= 2); - return R_AB_.col(index); - } - - /// In-place multiply of `this` rotation matrix `R_AB` by `other` rotation - /// matrix `R_BC`. On return, `this` is set to equal `R_AB * R_BC`. - /// @param[in] other %RotationMatrix that post-multiplies `this`. - /// @returns `this` rotation matrix which has been multiplied by `other`. - /// @note It is possible (albeit improbable) to create an invalid rotation - /// matrix by accumulating round-off error with a large number of multiplies. - RotationMatrix& operator*=(const RotationMatrix& other) { - if constexpr (std::is_same_v) { - internal::ComposeRR(*this, other, this); - } else { - SetUnchecked(matrix() * other.matrix()); - } - return *this; - } - - /// Calculates `this` rotation matrix `R_AB` multiplied by `other` rotation - /// matrix `R_BC`, returning the composition `R_AB * R_BC`. - /// @param[in] other %RotationMatrix that post-multiplies `this`. - /// @returns rotation matrix that results from `this` multiplied by `other`. - /// @note It is possible (albeit improbable) to create an invalid rotation - /// matrix by accumulating round-off error with a large number of multiplies. - RotationMatrix operator*(const RotationMatrix& other) const { - RotationMatrix R_AC(internal::DoNotInitializeMemberFields{}); - if constexpr (std::is_same_v) { - internal::ComposeRR(*this, other, &R_AC); - } else { - R_AC.R_AB_ = matrix() * other.matrix(); - } - return R_AC; - } - - /// Calculates the product of `this` inverted and another %RotationMatrix. - /// If you consider `this` to be the rotation matrix R_AB, and `other` to be - /// R_AC, then this method returns R_BC = R_AB⁻¹ * R_AC. For T==double, this - /// method can be _much_ faster than inverting first and then performing the - /// composition because it can take advantage of the orthogonality of - /// rotation matrices. On some platforms it can use SIMD instructions for - /// further speedups. - /// @param[in] other %RotationMatrix that post-multiplies `this` inverted. - /// @retval R_BC where R_BC = this⁻¹ * other. - /// @note It is possible (albeit improbable) to create an invalid rotation - /// matrix by accumulating round-off error with a large number of multiplies. - RotationMatrix InvertAndCompose(const RotationMatrix& other) const { - const RotationMatrix& R_AC = other; // Nicer name. - RotationMatrix R_BC(internal::DoNotInitializeMemberFields{}); - if constexpr (std::is_same_v) { - internal::ComposeRinvR(*this, R_AC, &R_BC); - } else { - const RotationMatrix R_BA = inverse(); - R_BC = R_BA * R_AC; - } - return R_BC; - } - - /// Calculates `this` rotation matrix `R_AB` multiplied by an arbitrary - /// Vector3 expressed in the B frame. - /// @param[in] v_B 3x1 vector that post-multiplies `this`. - /// @returns 3x1 vector `v_A = R_AB * v_B`. - Vector3 operator*(const Vector3& v_B) const { - return Vector3(matrix() * v_B); - } - - /// Multiplies `this` %RotationMatrix `R_AB` by the n vectors `v1`, ... `vn`, - /// where each vector has 3 elements and is expressed in frame B. - /// @param[in] v_B `3 x n` matrix whose n columns are regarded as arbitrary - /// vectors `v1`, ... `vn` expressed in frame B. - /// @retval v_A `3 x n` matrix whose n columns are vectors `v1`, ... `vn` - /// expressed in frame A. - /// @code{.cc} - /// const RollPitchYaw rpy(0.1, 0.2, 0.3); - /// const RotationMatrix R_AB(rpy); - /// Eigen::Matrix v_B; - /// v_B.col(0) = Vector3d(4, 5, 6); - /// v_B.col(1) = Vector3d(9, 8, 7); - /// const Eigen::Matrix v_A = R_AB * v_B; - /// @endcode - template - Eigen::Matrix - operator*(const Eigen::MatrixBase& v_B) const { - if (v_B.rows() != 3) { - throw std::logic_error( - "Error: Inner dimension for matrix multiplication is not 3."); - } - // Express vectors in terms of frame A as v_A = R_AB * v_B. - return matrix() * v_B; - } - - /// Returns how close the matrix R is to being a 3x3 orthonormal matrix by - /// computing `‖R ⋅ Rᵀ - I‖∞` (i.e., the maximum absolute value of the - /// difference between the elements of R ⋅ Rᵀ and the 3x3 identity matrix). - /// @param[in] R matrix being checked for orthonormality. - /// @returns `‖R ⋅ Rᵀ - I‖∞` - static T GetMeasureOfOrthonormality(const Matrix3& R) { - const Matrix3 m = R * R.transpose(); - return GetMaximumAbsoluteDifference(m, Matrix3::Identity()); - } - - /// Tests if a generic Matrix3 has orthonormal vectors to within the threshold - /// specified by `tolerance`. - /// @param[in] R an allegedly orthonormal rotation matrix. - /// @param[in] tolerance maximum allowable absolute difference between R * Rᵀ - /// and the identity matrix I, i.e., checks if `‖R ⋅ Rᵀ - I‖∞ <= tolerance`. - /// @returns `true` if R is an orthonormal matrix. - static boolean IsOrthonormal(const Matrix3& R, double tolerance) { - return GetMeasureOfOrthonormality(R) <= tolerance; - } - - /// Tests if a generic Matrix3 seems to be a proper orthonormal rotation - /// matrix to within the threshold specified by `tolerance`. - /// @param[in] R an allegedly valid rotation matrix. - /// @param[in] tolerance maximum allowable absolute difference of `R * Rᵀ` - /// and the identity matrix I (i.e., checks if `‖R ⋅ Rᵀ - I‖∞ <= tolerance`). - /// @returns `true` if R is a valid rotation matrix. - static boolean IsValid(const Matrix3& R, double tolerance) { - return IsOrthonormal(R, tolerance) && R.determinant() > 0; - } - - /// Tests if a generic Matrix3 is a proper orthonormal rotation matrix to - /// within the threshold of get_internal_tolerance_for_orthonormality(). - /// @param[in] R an allegedly valid rotation matrix. - /// @returns `true` if R is a valid rotation matrix. - static boolean IsValid(const Matrix3& R) { - return IsValid(R, get_internal_tolerance_for_orthonormality()); - } - - /// Tests if `this` rotation matrix R is a proper orthonormal rotation matrix - /// to within the threshold of get_internal_tolerance_for_orthonormality(). - /// @returns `true` if `this` is a valid rotation matrix. - boolean IsValid() const { return IsValid(matrix()); } - - /// Returns `true` if `this` is exactly equal to the identity matrix. - boolean IsExactlyIdentity() const { - return matrix() == Matrix3::Identity(); - } - - /// Returns true if `this` is equal to the identity matrix to within the - /// threshold of get_internal_tolerance_for_orthonormality(). - boolean IsIdentityToInternalTolerance() const { - return IsNearlyEqualTo(matrix(), Matrix3::Identity(), - get_internal_tolerance_for_orthonormality()); - } - - /// Compares each element of `this` to the corresponding element of `other` - /// to check if they are the same to within a specified `tolerance`. - /// @param[in] other %RotationMatrix to compare to `this`. - /// @param[in] tolerance maximum allowable absolute difference between the - /// matrix elements in `this` and `other`. - /// @returns `true` if `‖this - other‖∞ <= tolerance`. - boolean IsNearlyEqualTo(const RotationMatrix& other, - double tolerance) const { - return IsNearlyEqualTo(matrix(), other.matrix(), tolerance); - } - - /// Compares each element of `this` to the corresponding element of `other` - /// to check if they are exactly the same. - /// @param[in] other %RotationMatrix to compare to `this`. - /// @returns true if each element of `this` is exactly equal to the - /// corresponding element in `other`. - boolean IsExactlyEqualTo(const RotationMatrix& other) const { - return matrix() == other.matrix(); - } - - /// Computes the infinity norm of `this` - `other` (i.e., the maximum absolute - /// value of the difference between the elements of `this` and `other`). - /// @param[in] other %RotationMatrix to subtract from `this`. - /// @returns `‖this - other‖∞` - T GetMaximumAbsoluteDifference(const RotationMatrix& other) const { - return GetMaximumAbsoluteDifference(matrix(), other.matrix()); - } - - /// Given an approximate rotation matrix M, finds the %RotationMatrix R - /// closest to M. Closeness is measured with a matrix-2 norm (or equivalently - /// with a Frobenius norm). Hence, this method creates a %RotationMatrix R - /// from a 3x3 matrix M by minimizing `‖R - M‖₂` (the matrix-2 norm of (R-M)) - /// subject to `R * Rᵀ = I`, where I is the 3x3 identity matrix. For this - /// problem, closeness can also be measured by forming the orthonormal matrix - /// R whose elements minimize the double-summation `∑ᵢ ∑ⱼ (R(i,j) - M(i,j))²` - /// where `i = 1:3, j = 1:3`, subject to `R * Rᵀ = I`. The square-root of - /// this double-summation is called the Frobenius norm. - /// @param[in] M a 3x3 matrix. - /// @param[out] quality_factor. The quality of M as a rotation matrix. - /// `quality_factor` = 1 is perfect (M = R). `quality_factor` = 1.25 means - /// that when M multiplies a unit vector (magnitude 1), a vector of magnitude - /// as large as 1.25 may result. `quality_factor` = 0.8 means that when M - /// multiplies a unit vector, a vector of magnitude as small as 0.8 may - /// result. `quality_factor` = 0 means M is singular, so at least one of the - /// bases related by matrix M does not span 3D space (when M multiples a unit - /// vector, a vector of magnitude as small as 0 may result). - /// @returns proper orthonormal matrix R that is closest to M. - /// @throws std::exception if R fails IsValid(R). - /// @note William Kahan (UC Berkeley) and Hongkai Dai (Toyota Research - /// Institute) proved that for this problem, the same R that minimizes the - /// Frobenius norm also minimizes the matrix-2 norm (a.k.a an induced-2 norm), - /// which is defined [Dahleh, Section 4.2] as the column matrix u which - /// maximizes `‖(R - M) u‖ / ‖u‖`, where `u ≠ 0`. Since the matrix-2 norm of - /// any matrix A is equal to the maximum singular value of A, minimizing the - /// matrix-2 norm of (R - M) is equivalent to minimizing the maximum singular - /// value of (R - M). - /// - /// - [Dahleh] "Lectures on Dynamic Systems and Controls: Electrical - /// Engineering and Computer Science, Massachusetts Institute of Technology" - /// https://ocw.mit.edu/courses/electrical-engineering-and-computer-science/6-241j-dynamic-systems-and-control-spring-2011/readings/MIT6_241JS11_chap04.pdf - static RotationMatrix - ProjectToRotationMatrix(const Matrix3& M, T* quality_factor = nullptr) { - const Matrix3 M_orthonormalized = - ProjectMatrix3ToOrthonormalMatrix3(M, quality_factor); - ThrowIfNotValid(M_orthonormalized); - return RotationMatrix(M_orthonormalized, true); - } - - /// Returns an internal tolerance that checks rotation matrix orthonormality. - /// @returns internal tolerance (small multiplier of double-precision epsilon) - /// used to check whether or not a rotation matrix is orthonormal. - /// @note The tolerance is chosen by developers to ensure a reasonably - /// valid (orthonormal) rotation matrix. - /// @note To orthonormalize a 3x3 matrix, use ProjectToRotationMatrix(). - static constexpr double get_internal_tolerance_for_orthonormality() { - return kInternalToleranceForOrthonormality; - } - - /// Returns a quaternion q that represents `this` %RotationMatrix. Since the - /// quaternion `q` and `-q` represent the same %RotationMatrix, this method - /// chooses to return a canonical quaternion, i.e., with q(0) >= 0. - /// @note There is a constructor in the RollPitchYaw class that converts - /// a rotation matrix to roll-pitch-yaw angles. - Eigen::Quaternion ToQuaternion() const { return ToQuaternion(R_AB_); } - - /// Returns a unit quaternion q associated with the 3x3 matrix M. Since the - /// quaternion `q` and `-q` represent the same %RotationMatrix, this method - /// chooses to return a canonical quaternion, i.e., with q(0) >= 0. - /// @param[in] M 3x3 matrix to be made into a quaternion. - /// @returns a unit quaternion q in canonical form, i.e., with q(0) >= 0. - /// @throws std::exception in debug builds if the quaternion `q` - /// returned by this method cannot construct a valid %RotationMatrix. - /// For example, if `M` contains NaNs, `q` will not be a valid quaternion. - static Eigen::Quaternion ToQuaternion( - const Eigen::Ref>& M) { - Eigen::Quaternion q = RotationMatrixToUnnormalizedQuaternion(M); - - // Since the quaternions q and -q correspond to the same rotation matrix, - // choose to return a canonical quaternion, i.e., with q(0) >= 0. - const T canonical_factor = if_then_else(q.w() < 0, T(-1), T(1)); - - // The quantity q calculated thus far in this algorithm is not a quaternion - // with magnitude 1. It differs from a quaternion in that all elements of - // q are scaled by the same factor. To return a valid quaternion, q must be - // normalized so q(0)^2 + q(1)^2 + q(2)^2 + q(3)^2 = 1. - const T scale = canonical_factor / q.norm(); - q.coeffs() *= scale; - - MALIPUT_DRAKE_ASSERT_VOID(ThrowIfNotValid(QuaternionToRotationMatrix(q, T(2)))); - return q; - } - - /// Utility method to return the Vector4 associated with ToQuaternion(). - /// @see ToQuaternion(). - Vector4 ToQuaternionAsVector4() const { - return ToQuaternionAsVector4(R_AB_); - } - - /// Utility method to return the Vector4 associated with ToQuaternion(M). - /// @param[in] M 3x3 matrix to be made into a quaternion. - /// @see ToQuaternion(). - static Vector4 ToQuaternionAsVector4(const Matrix3& M) { - const Eigen::Quaternion q = ToQuaternion(M); - return Vector4(q.w(), q.x(), q.y(), q.z()); - } - - /// Returns an AngleAxis `theta_lambda` containing an angle `theta` and unit - /// vector (axis direction) `lambda` that represents `this` %RotationMatrix. - /// @note The orientation and %RotationMatrix associated with `theta * lambda` - /// is identical to that of `(-theta) * (-lambda)`. The AngleAxis returned by - /// this method chooses to have `0 <= theta <= pi`. - /// @returns an AngleAxis with `0 <= theta <= pi` and a unit vector `lambda`. - Eigen::AngleAxis ToAngleAxis() const { - const Eigen::AngleAxis theta_lambda(this->matrix()); - return theta_lambda; - } - - /// (Internal use only) Constructs a RotationMatrix without initializing the - /// underlying 3x3 matrix. Here for use by RigidTransform but no one else. - explicit RotationMatrix(internal::DoNotInitializeMemberFields) {} - - private: - // Make RotationMatrix templatized on any typename U be a friend of a - // %RotationMatrix templatized on any other typename T. - // This is needed for the method RotationMatrix::cast() to be able to - // use the necessary private constructor. - template - friend class RotationMatrix; - - // Declares the allowable tolerance (small multiplier of double-precision - // epsilon) used to check whether or not a rotation matrix is orthonormal. - static constexpr double kInternalToleranceForOrthonormality{ - 128 * std::numeric_limits::epsilon() }; - - - // Constructs a %RotationMatrix from a Matrix3. No check is performed to test - // whether or not the parameter R is a valid rotation matrix. - // @param[in] R an allegedly valid rotation matrix. - // @note The second parameter is just a dummy to distinguish this constructor - // from one of the public constructors. - RotationMatrix(const Matrix3& R, bool) : R_AB_(R) {} - - // Sets `this` %RotationMatrix from a Matrix3. No check is performed to - // test whether or not the parameter R is a valid rotation matrix. - // @param[in] R an allegedly valid rotation matrix. - void SetUnchecked(const Matrix3& R) { R_AB_ = R; } - - // Sets `this` %RotationMatrix `R_AB` from right-handed orthogonal unit - // vectors `Bx`, `By`, `Bz` so that the columns of `this` are `[Bx, By, Bz]`. - // @param[in] Bx first unit vector in right-handed orthogonal basis. - // @param[in] By second unit vector in right-handed orthogonal basis. - // @param[in] Bz third unit vector in right-handed orthogonal basis. - // @throws std::exception in debug builds if `R_AB` fails IsValid(R_AB). - // @note The rotation matrix `R_AB` relates two sets of right-handed - // orthogonal unit vectors, namely `Ax`, `Ay`, `Az` and `Bx`, `By`, `Bz`. - // The rows of `R_AB` are `Ax`, `Ay`, `Az` whereas the - // columns of `R_AB` are `Bx`, `By`, `Bz`. - void SetFromOrthonormalColumns(const Vector3& Bx, - const Vector3& By, - const Vector3& Bz) { - R_AB_.col(0) = Bx; - R_AB_.col(1) = By; - R_AB_.col(2) = Bz; - MALIPUT_DRAKE_ASSERT_VOID(ThrowIfNotValid(R_AB_)); - } - - // Sets `this` %RotationMatrix `R_AB` from right-handed orthogonal unit - // vectors `Ax`, `Ay`, `Az` so that the rows of `this` are `[Ax, Ay, Az]`. - // @param[in] Ax first unit vector in right-handed orthogonal basis. - // @param[in] Ay second unit vector in right-handed orthogonal basis. - // @param[in] Az third unit vector in right-handed orthogonal basis. - // @throws std::exception in debug builds if `R_AB` fails R_AB.IsValid(). - // @see SetFromOrthonormalColumns() for additional notes. - void SetFromOrthonormalRows(const Vector3& Ax, - const Vector3& Ay, - const Vector3& Az) { - R_AB_.row(0) = Ax; - R_AB_.row(1) = Ay; - R_AB_.row(2) = Az; - MALIPUT_DRAKE_ASSERT_VOID(ThrowIfNotValid(R_AB_)); - } - - // Computes the infinity norm of R - `other` (i.e., the maximum absolute - // value of the difference between the elements of R and `other`). - // @param[in] R matrix from which `other` is subtracted. - // @param[in] other matrix to subtract from R. - // @returns `‖R - other‖∞` - static T GetMaximumAbsoluteDifference(const Matrix3& R, - const Matrix3& other) { - const Matrix3 R_difference = R - other; - return R_difference.template lpNorm(); - } - - // Compares corresponding elements in two matrices to check if they are the - // same to within a specified `tolerance`. - // @param[in] R matrix to compare to `other`. - // @param[in] other matrix to compare to R. - // @param[in] tolerance maximum allowable absolute difference between the - // matrix elements in R and `other`. - // @returns `true` if `‖R - `other`‖∞ <= tolerance`. - static boolean IsNearlyEqualTo(const Matrix3& R, - const Matrix3& other, - double tolerance) { - const T R_max_difference = GetMaximumAbsoluteDifference(R, other); - return R_max_difference <= tolerance; - } - - // Throws an exception if R is not a valid %RotationMatrix. - // @param[in] R an allegedly valid rotation matrix. - // @note If the underlying scalar type T is non-numeric (symbolic), no - // validity check is made and no exception is thrown. - static void ThrowIfNotValid(const Matrix3& R); - - // Given an approximate rotation matrix M, finds the orthonormal matrix R - // closest to M. Closeness is measured with a matrix-2 norm (or equivalently - // with a Frobenius norm). Hence, this method creates an orthonormal matrix R - // from a 3x3 matrix M by minimizing `‖R - M‖₂` (the matrix-2 norm of (R-M)) - // subject to `R * Rᵀ = I`, where I is the 3x3 identity matrix. For this - // problem, closeness can also be measured by forming the orthonormal matrix R - // whose elements minimize the double-summation `∑ᵢ ∑ⱼ (R(i,j) - M(i,j))²` - // where `i = 1:3, j = 1:3`, subject to `R * Rᵀ = I`. The square-root of - // this double-summation is called the Frobenius norm. - // @param[in] M a 3x3 matrix. - // @param[out] quality_factor. The quality of M as a rotation matrix. - // `quality_factor` = 1 is perfect (M = R). `quality_factor` = 1.25 means - // that when M multiplies a unit vector (magnitude 1), a vector of magnitude - // as large as 1.25 may result. `quality_factor` = -1 means M relates two - // perfectly orthonormal bases, but one is right-handed whereas the other is - // left-handed (M is a "reflection"). `quality_factor` = -0.8 means M - // relates a right-handed basis to a left-handed basis and when M multiplies - // a unit vector, a vector of magnitude as small as 0.8 may result. - // `quality_factor` = 0 means M is singular, so at least one of the bases - // related by matrix M does not span 3D space (when M multiples a unit vector, - // a vector of magnitude as small as 0 may result). - // @return orthonormal matrix R that is closest to M (note det(R) may be -1). - // @note The SVD part of this algorithm can be derived as a modification of - // section 3.2 in http://haralick.org/conferences/pose_estimation.pdf. - // @note William Kahan (UC Berkeley) and Hongkai Dai (Toyota Research - // Institute) proved that for this problem, the same R that minimizes the - // Frobenius norm also minimizes the matrix-2 norm (a.k.a an induced-2 norm), - // which is defined [Dahleh, Section 4.2] as the column matrix u which - // maximizes `‖(R - M) u‖ / ‖u‖`, where `u ≠ 0`. Since the matrix-2 norm of - // any matrix A is equal to the maximum singular value of A, minimizing the - // matrix-2 norm of (R - M) is equivalent to minimizing the maximum singular - // value of (R - M). - // - // - [Dahleh] "Lectures on Dynamic Systems and Controls: Electrical - // Engineering and Computer Science, Massachusetts Institute of Technology" - // https://ocw.mit.edu/courses/electrical-engineering-and-computer-science/6-241j-dynamic-systems-and-control-spring-2011/readings/MIT6_241JS11_chap04.pdf - template - static Matrix3 ProjectMatrix3ToOrthonormalMatrix3( - const Eigen::MatrixBase& M, T* quality_factor) { - MALIPUT_DRAKE_DEMAND(M.rows() == 3 && M.cols() == 3); - const auto svd = M.jacobiSvd(Eigen::ComputeFullU | Eigen::ComputeFullV); - if (quality_factor != nullptr) { - // Singular values are always non-negative and sorted in decreasing order. - const auto singular_values = svd.singularValues(); - const T s_max = singular_values(0); // maximum singular value. - const T s_min = singular_values(2); // minimum singular value. - const T s_f = (s_max != 0.0 && s_min < 1.0/s_max) ? s_min : s_max; - const T det = M.determinant(); - const double sign_det = (det > 0.0) ? 1 : ((det < 0.0) ? -1 : 0); - *quality_factor = s_f * sign_det; - } - return svd.matrixU() * svd.matrixV().transpose(); - } - - // This is a helper method for RotationMatrix::ToQuaternion that returns a - // Quaternion that is neither sign-canonicalized nor magnitude-normalized. - // - // This method is used for scalar types where scalar_predicate::is_bool is - // true (e.g., T = `double` and T = `AutoDiffXd`). For types where is_bool - // is false (e.g., T = `symbolic::Expression`), the alternative specialization - // below is used instead. We have two implementations so that this method is - // as fast and compact as possible when `T = double`. - // - // N.B. Keep the math in this method in sync with the other specialization, - // immediately below. - template - static std::enable_if_t::is_bool, Eigen::Quaternion> - RotationMatrixToUnnormalizedQuaternion( - const Eigen::Ref>& M) { - // This implementation is adapted from simbody at - // https://github.com/simbody/simbody/blob/master/SimTKcommon/Mechanics/src/Rotation.cpp - T w, x, y, z; // Elements of the quaternion, w relates to cos(theta/2). - const T trace = M.trace(); - if (trace >= M(0, 0) && trace >= M(1, 1) && trace >= M(2, 2)) { - // This branch occurs if the trace is larger than any diagonal element. - w = T(1) + trace; - x = M(2, 1) - M(1, 2); - y = M(0, 2) - M(2, 0); - z = M(1, 0) - M(0, 1); - } else if (M(0, 0) >= M(1, 1) && M(0, 0) >= M(2, 2)) { - // This branch occurs if M(0,0) is largest among the diagonal elements. - w = M(2, 1) - M(1, 2); - x = T(1) - (trace - 2 * M(0, 0)); - y = M(0, 1) + M(1, 0); - z = M(0, 2) + M(2, 0); - } else if (M(1, 1) >= M(2, 2)) { - // This branch occurs if M(1,1) is largest among the diagonal elements. - w = M(0, 2) - M(2, 0); - x = M(0, 1) + M(1, 0); - y = T(1) - (trace - 2 * M(1, 1)); - z = M(1, 2) + M(2, 1); - } else { - // This branch occurs if M(2,2) is largest among the diagonal elements. - w = M(1, 0) - M(0, 1); - x = M(0, 2) + M(2, 0); - y = M(1, 2) + M(2, 1); - z = T(1) - (trace - 2 * M(2, 2)); - } - // Create a quantity q (which is not yet a unit quaternion). - // Note: Eigen's Quaternion constructor does not normalize. - return Eigen::Quaternion(w, x, y, z); - } - - // Refer to the same-named method above for comments and details about the - // algorithm being used, the meaning of the branches, etc. This method is - // identical except that the if-elseif chain is replaced with a symbolic- - // conditional formulation. - // - // N.B. Keep the math in this method in sync with the other specialization, - // immediately above. - template - static std::enable_if_t::is_bool, Eigen::Quaternion> - RotationMatrixToUnnormalizedQuaternion( - const Eigen::Ref>& M) { - const T M00 = M(0, 0); const T M01 = M(0, 1); const T M02 = M(0, 2); - const T M10 = M(1, 0); const T M11 = M(1, 1); const T M12 = M(1, 2); - const T M20 = M(2, 0); const T M21 = M(2, 1); const T M22 = M(2, 2); - const T trace = M00 + M11 + M22; - auto if_then_else_vec4 = []( - const boolean& f_cond, - const Vector4& e_then, - const Vector4& e_else) -> Vector4 { - return Vector4( - if_then_else(f_cond, e_then[0], e_else[0]), - if_then_else(f_cond, e_then[1], e_else[1]), - if_then_else(f_cond, e_then[2], e_else[2]), - if_then_else(f_cond, e_then[3], e_else[3])); - }; - const Vector4 wxyz = - if_then_else_vec4(trace >= M00 && trace >= M11 && trace >= M22, { - T(1) + trace, - M21 - M12, - M02 - M20, - M10 - M01, - }, if_then_else_vec4(M00 >= M11 && M00 >= M22, { - M21 - M12, - T(1) - (trace - 2 * M00), - M01 + M10, - M02 + M20, - }, if_then_else_vec4(M11 >= M22, { - M02 - M20, - M01 + M10, - T(1) - (trace - 2 * M11), - M12 + M21, - }, /* else */ { - M10 - M01, - M02 + M20, - M12 + M21, - T(1) - (trace - 2 * M22), - }))); - return Eigen::Quaternion(wxyz(0), wxyz(1), wxyz(2), wxyz(3)); - } - - // Constructs a 3x3 rotation matrix from a Quaternion. - // @param[in] quaternion a quaternion which may or may not have unit length. - // @param[in] two_over_norm_squared is supplied by the calling method and is - // usually pre-computed as `2 / quaternion.squaredNorm()`. If `quaternion` - // has already been normalized [`quaternion.norm() = 1`] or there is a reason - // (unlikely) that the calling method determines that normalization is - // unwanted, the calling method should just past `two_over_norm_squared = 2`. - // @internal The cost of Eigen's quaternion.toRotationMatrix() is 12 adds and - // 12 multiplies. This method also costs 12 adds and 12 multiplies, but - // has a provision for an efficient algorithm for always calculating an - // orthogonal rotation matrix (whereas Eigen's algorithm does not). - static Matrix3 QuaternionToRotationMatrix( - const Eigen::Quaternion& quaternion, const T& two_over_norm_squared) { - Matrix3 m; - - const T w = quaternion.w(); - const T x = quaternion.x(); - const T y = quaternion.y(); - const T z = quaternion.z(); - const T sx = two_over_norm_squared * x; // scaled x-value. - const T sy = two_over_norm_squared * y; // scaled y-value. - const T sz = two_over_norm_squared * z; // scaled z-value. - const T swx = sx * w; - const T swy = sy * w; - const T swz = sz * w; - const T sxx = sx * x; - const T sxy = sy * x; - const T sxz = sz * x; - const T syy = sy * y; - const T syz = sz * y; - const T szz = sz * z; - - m.coeffRef(0, 0) = T(1) - syy - szz; - m.coeffRef(0, 1) = sxy - swz; - m.coeffRef(0, 2) = sxz + swy; - m.coeffRef(1, 0) = sxy + swz; - m.coeffRef(1, 1) = T(1) - sxx - szz; - m.coeffRef(1, 2) = syz - swx; - m.coeffRef(2, 0) = sxz - swy; - m.coeffRef(2, 1) = syz + swx; - m.coeffRef(2, 2) = T(1) - sxx - syy; - - return m; - } - - // Throws an exception if the vector v does not have a measurable magnitude - // within 4ε of 1 (where machine epsilon ε ≈ 2.22E-16). - // @param[in] v The vector to test. - // @param[in] function_name The name of the calling function; included in the - // exception message. - // @throws std::exception if |v| cannot be verified to be within 4ε of 1. - // An exception is thrown if v contains nonfinite numbers (NaN or infinity). - // @note no exception is thrown if v is a symbolic type. - static void ThrowIfNotUnitLength(const Vector3& v, - const char* function_name); - - // Returns the unit vector in the direction of v or throws an exception if v - // cannot be "safely" normalized. - // @param[in] v The vector to normalize. - // @param[in] function_name The name of the calling function; included in the - // exception message. - // @throws std::exception if v contains nonfinite numbers (NaN or infinity) - // or |v| < 1E-10. - // @note no exception is thrown if v is a symbolic type. - static Vector3 NormalizeOrThrow(const Vector3& v, - const char* function_name); - - // Stores the underlying rotation matrix relating two frames (e.g. A and B). - // For speed, `R_AB_` is uninitialized (public constructors set its value). - // The elements are stored in column-major order, per Eigen's default, - // see https://eigen.tuxfamily.org/dox/group__TopicStorageOrders.html. - Matrix3 R_AB_; -}; - -// To enable low-level optimizations we insist that RotationMatrix is -// packed into 9 consecutive doubles, with no extra alignment padding. -static_assert(sizeof(RotationMatrix) == 9 * sizeof(double), - "Low-level optimizations depend on RotationMatrix being stored as " - "9 sequential doubles in memory, with no extra memory alignment padding."); - -/// Abbreviation (alias/typedef) for a RotationMatrix double scalar type. -/// @relates RotationMatrix -using RotationMatrixd = RotationMatrix; - -/// Projects an approximate 3 x 3 rotation matrix M onto an orthonormal matrix R -/// so that R is a rotation matrix associated with a angle-axis rotation by an -/// angle θ about a vector direction `axis`, with `angle_lb <= θ <= angle_ub`. -/// @param[in] M the matrix to be projected. -/// @param[in] axis vector direction associated with angle-axis rotation for R. -/// axis can be a non-unit vector, but cannot be the zero vector. -/// @param[in] angle_lb the lower bound of the rotation angle θ. -/// @param[in] angle_ub the upper bound of the rotation angle θ. -/// @return Rotation angle θ of the projected matrix, angle_lb <= θ <= angle_ub -/// @throws std::exception if axis is the zero vector or -/// if angle_lb > angle_ub. -/// @note This method is useful for reconstructing a rotation matrix for a -/// revolute joint with joint limits. -/// @note This can be formulated as an optimization problem -/// 
-///   min_θ trace((R - M)ᵀ*(R - M))
-///   subject to R = I + sinθ * A + (1 - cosθ) * A²   (1)
-///              angle_lb <= θ <= angle_ub
-///
-/// where A is the cross product matrix of a = axis / axis.norm() = [a₁, a₂, a₃] -/// 
-/// A = [ 0  -a₃  a₂]
-///     [ a₃  0  -a₁]
-///     [-a₂  a₁  0 ]
-///
-/// Equation (1) is the Rodriguez Formula that computes the rotation matrix R -/// from the angle-axis rotation with angle θ and vector direction `axis`. -/// For details, see http://mathworld.wolfram.com/RodriguesRotationFormula.html -/// The objective function can be simplified as -/// 
-///    max_θ trace(Rᵀ * M + Mᵀ * R)
-///
-/// By substituting the matrix `R` with the angle-axis representation, the -/// optimization problem is formulated as -/// 
-///    max_θ sinθ * trace(Aᵀ*M) - cosθ * trace(Mᵀ * A²)
-///    subject to angle_lb <= θ <= angle_ub
-///
-/// By introducing α = atan2(-trace(Mᵀ * A²), trace(Aᵀ*M)), we can compute the -/// optimal θ as -/// 
-///    θ = π/2 + 2kπ - α, if angle_lb <= π/2 + 2kπ - α <= angle_ub, k ∈ integers
-/// else
-///    θ = angle_lb, if sin(angle_lb + α) >= sin(angle_ub + α)
-///    θ = angle_ub, if sin(angle_lb + α) <  sin(angle_ub + α)
-///
-/// @see GlobalInverseKinematics for an usage of this function. -double ProjectMatToRotMatWithAxis(const Eigen::Matrix3d& M, - const Eigen::Vector3d& axis, - double angle_lb, - double angle_ub); - -} // namespace math -} // namespace maliput::drake - -DRAKE_DECLARE_CLASS_TEMPLATE_INSTANTIATIONS_ON_DEFAULT_SCALARS( - class ::maliput::drake::math::RotationMatrix) diff --git a/include/maliput/drake/math/saturate.h b/include/maliput/drake/math/saturate.h deleted file mode 100644 index a30a92a..0000000 --- a/include/maliput/drake/math/saturate.h +++ /dev/null @@ -1,23 +0,0 @@ -#pragma once - -// TODO(jwnimmer-tri): Figure out how to remove this include. -#include "maliput/drake/common/autodiff.h" -#include "maliput/drake/common/cond.h" -#include "maliput/drake/common/drake_assert.h" - -namespace maliput::drake { -namespace math { - -/// Saturates the input `value` between upper and lower bounds. If `value` is -/// within `[low, high]` then return it; else return the boundary. -template -T1 saturate(const T1& value, const T2& low, const T3& high) { - MALIPUT_DRAKE_ASSERT(low <= high); - return cond( - value < low, low, - value > high, high, - value); -} - -} // namespace math -} // namespace maliput::drake diff --git a/include/maliput/drake/math/wrap_to.h b/include/maliput/drake/math/wrap_to.h deleted file mode 100644 index f7a54ed..0000000 --- a/include/maliput/drake/math/wrap_to.h +++ /dev/null @@ -1,33 +0,0 @@ -#pragma once - -#include - -#include "maliput/drake/common/double_overloads.h" -#include "maliput/drake/common/drake_assert.h" - -namespace maliput::drake { -namespace math { - -/// For variables that are meant to be periodic, (e.g. over a 2π interval), -/// wraps `value` into the interval `[low, high)`. Precisely, `wrap_to` -/// returns: -/// value + k*(high-low) -/// for the unique integer value `k` that lands the output in the desired -/// interval. @p low and @p high must be finite, and low < high. -/// -template -T1 wrap_to(const T1& value, const T2& low, const T2& high) { - MALIPUT_DRAKE_ASSERT(low < high); - const T2 range = high - low; - return value - range * floor((value - low) / range); - // TODO(russt): jwnimmer preferred the following implementation (which may be - // numerically better), but fmod is not supported yet by autodiff nor - // symbolic: - // using std::fmod; - // const T1 rem = fmod(value - low, high - low); - // return if_then_else(rem >= T1(0), low + rem, high + rem); -} - - -} // namespace math -} // namespace maliput::drake diff --git a/include/maliput/drake/systems/analysis/antiderivative_function.h b/include/maliput/drake/systems/analysis/antiderivative_function.h index 887db5c..73393c5 100644 --- a/include/maliput/drake/systems/analysis/antiderivative_function.h +++ b/include/maliput/drake/systems/analysis/antiderivative_function.h @@ -1,5 +1,7 @@ #pragma once +#define MALIPUT_USED + #include #include #include diff --git a/include/maliput/drake/systems/analysis/dense_output.h b/include/maliput/drake/systems/analysis/dense_output.h index 6b92964..1bcee52 100644 --- a/include/maliput/drake/systems/analysis/dense_output.h +++ b/include/maliput/drake/systems/analysis/dense_output.h @@ -1,5 +1,7 @@ #pragma once +#define MALIPUT_USED + #include #include diff --git a/include/maliput/drake/systems/analysis/explicit_euler_integrator.h b/include/maliput/drake/systems/analysis/explicit_euler_integrator.h deleted file mode 100644 index 50c6259..0000000 --- a/include/maliput/drake/systems/analysis/explicit_euler_integrator.h +++ /dev/null @@ -1,97 +0,0 @@ -#pragma once - -#include - -#include "maliput/drake/common/default_scalars.h" -#include "maliput/drake/common/drake_copyable.h" -#include "maliput/drake/systems/analysis/integrator_base.h" - -namespace maliput::drake { -namespace systems { - -/** - * A first-order, explicit Euler integrator. State is updated in the following - * manner: - * 
- * x(t+h) = x(t) + dx/dt * h
- *
- * - * @tparam_default_scalar - * @ingroup integrators - */ -template -class ExplicitEulerIntegrator final : public IntegratorBase { - public: - DRAKE_NO_COPY_NO_MOVE_NO_ASSIGN(ExplicitEulerIntegrator) - - ~ExplicitEulerIntegrator() override = default; - - /** - * Constructs a fixed-step integrator for a given system using the given - * context for initial conditions. - * @param system A reference to the system to be simulated - * @param max_step_size The maximum (fixed) step size; the integrator will - * not take larger step sizes than this. - * @param context Pointer to the context (nullptr is ok, but the caller - * must set a non-null context before Initialize()-ing the - * integrator). - * @sa Initialize() - */ - ExplicitEulerIntegrator(const System& system, const T& max_step_size, - Context* context = nullptr) - : IntegratorBase(system, context) { - IntegratorBase::set_maximum_step_size(max_step_size); - } - - /** - * Explicit Euler integrator does not support error estimation. - */ - bool supports_error_estimation() const override { return false; } - - /// Integrator does not provide an error estimate. - int get_error_estimate_order() const override { return 0; } - - private: - bool DoStep(const T& h) override; -}; - -/** - * Integrates the system forward in time by h, starting at the current time t₀. - * This value of h is determined by IntegratorBase::Step(). - */ -template -bool ExplicitEulerIntegrator::DoStep(const T& h) { - Context& context = *this->get_mutable_context(); - - // CAUTION: This is performance-sensitive inner loop code that uses dangerous - // long-lived references into state and cache to avoid unnecessary copying and - // cache invalidation. Be careful not to insert calls to methods that could - // invalidate any of these references before they are used. - - // Evaluate derivative xcdot₀ ← xcdot(t₀, x(t₀), u(t₀)). - const ContinuousState& xc_deriv = this->EvalTimeDerivatives(context); - const VectorBase& xcdot0 = xc_deriv.get_vector(); - - // Cache: xcdot0 references the live derivative cache value, currently - // up to date but about to be marked out of date. We do not want to make - // an unnecessary copy of this data. - - // Update continuous state and time. This call marks t- and xc-dependent - // cache entries out of date, including xcdot0. - VectorBase& xc = context.SetTimeAndGetMutableContinuousStateVector( - context.get_time() + h); // t ← t₀ + h - - // Cache: xcdot0 still references the derivative cache value, which is - // unchanged, although it is marked out of date. - - xc.PlusEqScaled(h, xcdot0); // xc(t₀ + h) ← xc(t₀) + h * xcdot₀ - - // This integrator always succeeds at taking the step. - return true; -} - -} // namespace systems -} // namespace maliput::drake - -DRAKE_DECLARE_CLASS_TEMPLATE_INSTANTIATIONS_ON_DEFAULT_SCALARS( - class maliput::drake::systems::ExplicitEulerIntegrator) diff --git a/include/maliput/drake/systems/analysis/hermitian_dense_output.h b/include/maliput/drake/systems/analysis/hermitian_dense_output.h index faab992..8c0a34e 100644 --- a/include/maliput/drake/systems/analysis/hermitian_dense_output.h +++ b/include/maliput/drake/systems/analysis/hermitian_dense_output.h @@ -1,18 +1,21 @@ #pragma once + +#define MALIPUT_USED + #include #include #include #include #include -#include "maliput/drake/common/autodiff.h" +// #include "maliput/drake/common/autodiff.h" #include "maliput/drake/common/default_scalars.h" #include "maliput/drake/common/drake_bool.h" #include "maliput/drake/common/drake_copyable.h" #include "maliput/drake/common/eigen_types.h" #include "maliput/drake/common/extract_double.h" -#include "maliput/drake/common/symbolic.h" +// #include "maliput/drake/common/symbolic.h" #include "maliput/drake/common/trajectories/piecewise_polynomial.h" #include "maliput/drake/systems/analysis/stepwise_dense_output.h" diff --git a/include/maliput/drake/systems/analysis/initial_value_problem.h b/include/maliput/drake/systems/analysis/initial_value_problem.h index 7e47c2f..c97372f 100644 --- a/include/maliput/drake/systems/analysis/initial_value_problem.h +++ b/include/maliput/drake/systems/analysis/initial_value_problem.h @@ -1,5 +1,7 @@ #pragma once +#define MALIPUT_USED + #include #include #include diff --git a/include/maliput/drake/systems/analysis/integrator_base.h b/include/maliput/drake/systems/analysis/integrator_base.h index 984307d..5b852fe 100644 --- a/include/maliput/drake/systems/analysis/integrator_base.h +++ b/include/maliput/drake/systems/analysis/integrator_base.h @@ -1,5 +1,7 @@ #pragma once +#define MALIPUT_USED + #include #include #include diff --git a/include/maliput/drake/systems/analysis/runge_kutta3_integrator.h b/include/maliput/drake/systems/analysis/runge_kutta3_integrator.h index e3311f7..3e78d57 100644 --- a/include/maliput/drake/systems/analysis/runge_kutta3_integrator.h +++ b/include/maliput/drake/systems/analysis/runge_kutta3_integrator.h @@ -1,5 +1,7 @@ #pragma once +#define MALIPUT_USED + #include #include diff --git a/include/maliput/drake/systems/analysis/scalar_dense_output.h b/include/maliput/drake/systems/analysis/scalar_dense_output.h index 589dc90..fdd2cdc 100644 --- a/include/maliput/drake/systems/analysis/scalar_dense_output.h +++ b/include/maliput/drake/systems/analysis/scalar_dense_output.h @@ -1,5 +1,7 @@ #pragma once +#define MALIPUT_USED + #include "maliput/drake/common/default_scalars.h" #include "maliput/drake/common/drake_copyable.h" #include "maliput/drake/systems/analysis/dense_output.h" diff --git a/include/maliput/drake/systems/analysis/scalar_initial_value_problem.h b/include/maliput/drake/systems/analysis/scalar_initial_value_problem.h index 1b4f6b4..08a6f4b 100644 --- a/include/maliput/drake/systems/analysis/scalar_initial_value_problem.h +++ b/include/maliput/drake/systems/analysis/scalar_initial_value_problem.h @@ -1,5 +1,7 @@ #pragma once +#define MALIPUT_USED + #include #include #include diff --git a/include/maliput/drake/systems/analysis/scalar_view_dense_output.h b/include/maliput/drake/systems/analysis/scalar_view_dense_output.h index 6c9d25c..8b50d89 100644 --- a/include/maliput/drake/systems/analysis/scalar_view_dense_output.h +++ b/include/maliput/drake/systems/analysis/scalar_view_dense_output.h @@ -1,5 +1,8 @@ #pragma once + +#define MALIPUT_USED + #include #include diff --git a/include/maliput/drake/systems/analysis/stepwise_dense_output.h b/include/maliput/drake/systems/analysis/stepwise_dense_output.h index 894d591..c9ef09f 100644 --- a/include/maliput/drake/systems/analysis/stepwise_dense_output.h +++ b/include/maliput/drake/systems/analysis/stepwise_dense_output.h @@ -1,5 +1,7 @@ #pragma once +#define MALIPUT_USED + #include "maliput/drake/common/default_scalars.h" #include "maliput/drake/common/drake_copyable.h" #include "maliput/drake/systems/analysis/dense_output.h" diff --git a/include/maliput/drake/systems/framework/abstract_value_cloner.h b/include/maliput/drake/systems/framework/abstract_value_cloner.h index a0fd18c..9fefe87 100644 --- a/include/maliput/drake/systems/framework/abstract_value_cloner.h +++ b/include/maliput/drake/systems/framework/abstract_value_cloner.h @@ -1,5 +1,8 @@ #pragma once +#define MALIPUT_USED + + #include #include "maliput/drake/common/copyable_unique_ptr.h" diff --git a/include/maliput/drake/systems/framework/abstract_values.h b/include/maliput/drake/systems/framework/abstract_values.h index 152e957..5714c2e 100644 --- a/include/maliput/drake/systems/framework/abstract_values.h +++ b/include/maliput/drake/systems/framework/abstract_values.h @@ -1,4 +1,5 @@ #pragma once +#define MALIPUT_USED #include #include diff --git a/include/maliput/drake/systems/framework/basic_vector.h b/include/maliput/drake/systems/framework/basic_vector.h index a2833bd..0a4fe60 100644 --- a/include/maliput/drake/systems/framework/basic_vector.h +++ b/include/maliput/drake/systems/framework/basic_vector.h @@ -1,4 +1,5 @@ #pragma once +#define MALIPUT_USED #include #include diff --git a/include/maliput/drake/systems/framework/context.h b/include/maliput/drake/systems/framework/context.h index 8d52b46..77c5689 100644 --- a/include/maliput/drake/systems/framework/context.h +++ b/include/maliput/drake/systems/framework/context.h @@ -1,5 +1,7 @@ #pragma once +#define MALIPUT_USED + #include #include #include diff --git a/include/maliput/drake/systems/framework/context_base.h b/include/maliput/drake/systems/framework/context_base.h index ea1e2cf..97b9c35 100644 --- a/include/maliput/drake/systems/framework/context_base.h +++ b/include/maliput/drake/systems/framework/context_base.h @@ -1,5 +1,7 @@ #pragma once +#define MALIPUT_USED + #include #include #include diff --git a/include/maliput/drake/systems/framework/continuous_state.h b/include/maliput/drake/systems/framework/continuous_state.h index ba99d5a..2736a6c 100644 --- a/include/maliput/drake/systems/framework/continuous_state.h +++ b/include/maliput/drake/systems/framework/continuous_state.h @@ -1,5 +1,7 @@ #pragma once +#define MALIPUT_USED + #include #include "maliput/drake/common/default_scalars.h" diff --git a/include/maliput/drake/systems/framework/discrete_values.h b/include/maliput/drake/systems/framework/discrete_values.h index ff7ac8d..3374ff4 100644 --- a/include/maliput/drake/systems/framework/discrete_values.h +++ b/include/maliput/drake/systems/framework/discrete_values.h @@ -1,4 +1,5 @@ #pragma once +#define MALIPUT_USED #include #include diff --git a/include/maliput/drake/systems/framework/framework_common.h b/include/maliput/drake/systems/framework/framework_common.h index b10413f..6865c4f 100644 --- a/include/maliput/drake/systems/framework/framework_common.h +++ b/include/maliput/drake/systems/framework/framework_common.h @@ -1,5 +1,7 @@ #pragma once +#define MALIPUT_USED + #include #include #include diff --git a/include/maliput/drake/systems/framework/leaf_output_port.h b/include/maliput/drake/systems/framework/leaf_output_port.h index 709e392..f0615fd 100644 --- a/include/maliput/drake/systems/framework/leaf_output_port.h +++ b/include/maliput/drake/systems/framework/leaf_output_port.h @@ -1,4 +1,5 @@ #pragma once +#define MALIPUT_USED #include #include diff --git a/include/maliput/drake/systems/framework/leaf_system.h b/include/maliput/drake/systems/framework/leaf_system.h index b285bbc..0b8f631 100644 --- a/include/maliput/drake/systems/framework/leaf_system.h +++ b/include/maliput/drake/systems/framework/leaf_system.h @@ -1,5 +1,7 @@ #pragma once +#define MALIPUT_USED + #include #include #include diff --git a/include/maliput/drake/systems/framework/model_values.h b/include/maliput/drake/systems/framework/model_values.h index aab6602..017c8cc 100644 --- a/include/maliput/drake/systems/framework/model_values.h +++ b/include/maliput/drake/systems/framework/model_values.h @@ -1,4 +1,5 @@ #pragma once +#define MALIPUT_USED #include #include diff --git a/include/maliput/drake/systems/framework/output_port.h b/include/maliput/drake/systems/framework/output_port.h index 49ff8cc..69ca311 100644 --- a/include/maliput/drake/systems/framework/output_port.h +++ b/include/maliput/drake/systems/framework/output_port.h @@ -8,7 +8,7 @@ #include -#include "maliput/drake/common/autodiff.h" +// #include "maliput/drake/common/autodiff.h" #include "maliput/drake/common/default_scalars.h" #include "maliput/drake/common/drake_assert.h" #include "maliput/drake/common/drake_deprecated.h" diff --git a/include/maliput/drake/systems/framework/parameters.h b/include/maliput/drake/systems/framework/parameters.h index 38a5483..cb60076 100644 --- a/include/maliput/drake/systems/framework/parameters.h +++ b/include/maliput/drake/systems/framework/parameters.h @@ -1,5 +1,7 @@ #pragma once +#define MALIPUT_USED + #include #include #include diff --git a/include/maliput/drake/systems/framework/scalar_conversion_traits.h b/include/maliput/drake/systems/framework/scalar_conversion_traits.h index c8da7b6..c54858f 100644 --- a/include/maliput/drake/systems/framework/scalar_conversion_traits.h +++ b/include/maliput/drake/systems/framework/scalar_conversion_traits.h @@ -1,5 +1,7 @@ #pragma once +#define MALIPUT_USED + #include #include "maliput/drake/common/symbolic.h" diff --git a/include/maliput/drake/systems/framework/state.h b/include/maliput/drake/systems/framework/state.h index 4dfccd7..458238e 100644 --- a/include/maliput/drake/systems/framework/state.h +++ b/include/maliput/drake/systems/framework/state.h @@ -1,5 +1,7 @@ #pragma once +#define MALIPUT_USED + #include #include diff --git a/include/maliput/drake/systems/framework/system.h b/include/maliput/drake/systems/framework/system.h index 61e6088..a87ae9d 100644 --- a/include/maliput/drake/systems/framework/system.h +++ b/include/maliput/drake/systems/framework/system.h @@ -1,5 +1,7 @@ #pragma once +#define MALIPUT_USED + #include #include #include @@ -1065,7 +1067,7 @@ class System : public SystemBase { See @ref system_scalar_conversion for detailed background and examples related to scalar-type conversion support. */ - std::unique_ptr> ToAutoDiffXd() const; + // std::unique_ptr> ToAutoDiffXd() const; /** Creates a deep copy of `from`, transmogrified to use the autodiff scalar type, with a dynamic-sized vector of partial derivatives. The result is @@ -1082,15 +1084,15 @@ class System : public SystemBase { See @ref system_scalar_conversion for detailed background and examples related to scalar-type conversion support. */ - template