From 5e7cbd551e69185c8f590433a784142492482697 Mon Sep 17 00:00:00 2001 From: Murilo Marinho Date: Fri, 4 Sep 2026 09:55:27 +0100 Subject: [PATCH] Add doxygen-compliant documentation based on the MATLAB docs. --- include/dqrobotics/DQ.h | 327 ++++++++++++++- include/dqrobotics/internal/_dq_linesegment.h | 82 +++- .../robot_control/DQ_ClassicQPController.h | 36 ++ .../DQ_KinematicConstrainedController.h | 35 ++ .../robot_control/DQ_KinematicController.h | 199 ++++++++- ...NumericalFilteredPseudoInverseController.h | 81 ++++ .../DQ_PseudoinverseController.h | 36 ++ .../DQ_QuadraticProgrammingController.h | 69 ++++ .../DQ_CooperativeDualTaskSpace.h | 73 ++++ .../DQ_DifferentialDriveRobot.h | 68 ++++ .../robot_modeling/DQ_HolonomicBase.h | 85 ++++ .../dqrobotics/robot_modeling/DQ_JointType.h | 61 ++- .../dqrobotics/robot_modeling/DQ_Kinematics.h | 352 ++++++++++++++++ .../dqrobotics/robot_modeling/DQ_MobileBase.h | 25 ++ .../robot_modeling/DQ_ParameterDH.h | 63 ++- .../robot_modeling/DQ_SerialManipulator.h | 385 ++++++++++++++---- .../robot_modeling/DQ_SerialManipulatorDH.h | 118 +++++- .../DQ_SerialManipulatorDenso.h | 84 ++++ .../robot_modeling/DQ_SerialManipulatorMDH.h | 118 +++++- .../robot_modeling/DQ_SerialWholeBody.h | 167 ++++++++ .../dqrobotics/robot_modeling/DQ_WholeBody.h | 120 ++++++ .../dqrobotics/robots/Ax18ManipulatorRobot.h | 12 + .../dqrobotics/robots/BarrettWamArmRobot.h | 12 + .../dqrobotics/robots/ComauSmartSixRobot.h | 12 + .../dqrobotics/robots/FrankaEmikaPandaRobot.h | 14 +- include/dqrobotics/robots/KukaLw4Robot.h | 12 + include/dqrobotics/robots/KukaYoubotRobot.h | 14 + .../solvers/DQ_QuadraticProgrammingSolver.h | 27 ++ include/dqrobotics/utils/DQ_Constants.h | 6 + include/dqrobotics/utils/DQ_Geometry.h | 143 +++++++ include/dqrobotics/utils/DQ_LinearAlgebra.h | 33 ++ include/dqrobotics/utils/DQ_Math.h | 28 ++ 32 files changed, 2752 insertions(+), 145 deletions(-) diff --git a/include/dqrobotics/DQ.h b/include/dqrobotics/DQ.h index 419bb3c..fc68b57 100644 --- a/include/dqrobotics/DQ.h +++ b/include/dqrobotics/DQ.h @@ -49,7 +49,26 @@ using namespace Eigen; namespace DQ_robotics{ - +/** + * @brief Units, operations, and operators related to dual quaternions. + * + * A dual quaternion is represented internally as an eight-dimensional + * vector `q`, ordered as (real part, imaginary i, j, k, and dual part + * real, i, j, k components). A dual quaternion can be constructed from the + * dual unit `DQ::E` and the imaginary units `DQ::i`, `DQ::j`, `DQ::k` + * (e.g., `DQ dq = 1 + 2*DQ::i + 5*DQ::E*DQ::k;`), or directly from a vector + * of 8, 6, 4, 3, or 1 dimensions containing the dual quaternion + * coefficients: + * - An eight-dimensional vector contains the coefficients of a general + * dual quaternion. + * - A six-dimensional vector contains the coefficients of a pure dual + * quaternion. + * - A four-dimensional vector contains the coefficients of a general + * quaternion. + * - A three-dimensional vector contains the coefficients of a pure + * quaternion. + * - A one-dimensional vector contains the coefficient of a real number. + */ class DQ{ private: @@ -58,9 +77,22 @@ class DQ{ public: //Member - Matrix q; + Matrix q; ///< The eight-dimensional vector of coefficients of this dual quaternion. //Static + /** + * @brief Builds the unit dual quaternion representing a rotation of + * @p rot_angle radians around the axis (@p x_axis, @p y_axis, @p z_axis) + * combined with the translation (@p x_trans, @p y_trans, @p z_trans). + * @param rot_angle The rotation angle, in radians. + * @param x_axis The x component of the rotation axis (pure quaternion). + * @param y_axis The y component of the rotation axis (pure quaternion). + * @param z_axis The z component of the rotation axis (pure quaternion). + * @param x_trans The x component of the translation. + * @param y_trans The y component of the translation. + * @param z_trans The z component of the translation. + * @return The corresponding unit dual quaternion. + */ static DQ unitDQ( const double& rot_angle, const int& x_axis, const int& y_axis, @@ -69,15 +101,33 @@ class DQ{ const double& y_trans, const double& z_trans); //To comply with MATLAB - const static DQ i; - const static DQ j; - const static DQ k; - const static DQ E; + const static DQ i; ///< The imaginary unit i. + const static DQ j; ///< The imaginary unit j. + const static DQ k; ///< The imaginary unit k. + const static DQ E; ///< The dual unit. //Constructors + /// @brief Constructs a dual quaternion from a vector of 8, 6, 4, 3, or 1 + /// dimensions containing the dual quaternion coefficients. explicit DQ(VectorXd&& v); + /** + * @brief Constructs a dual quaternion from a vector of 8, 6, 4, 3, or 1 + * dimensions containing the dual quaternion coefficients. + * @param v The vector containing the dual quaternion coefficients. + */ explicit DQ(const VectorXd& v); + /** + * @brief Constructs a dual quaternion from its eight coefficients. + * @param q0 Real part of the real component. + * @param q1 Coefficient of the imaginary unit i of the real component. + * @param q2 Coefficient of the imaginary unit j of the real component. + * @param q3 Coefficient of the imaginary unit k of the real component. + * @param q4 Real part of the dual component. + * @param q5 Coefficient of the imaginary unit i of the dual component. + * @param q6 Coefficient of the imaginary unit j of the dual component. + * @param q7 Coefficient of the imaginary unit k of the dual component. + */ explicit DQ(const double& q0=0.0, const double& q1=0.0, const double& q2=0.0, @@ -88,79 +138,190 @@ class DQ{ const double& q7=0.0) noexcept; //Member functions + /// @brief Returns the primary part of this dual quaternion. DQ P() const; + /// @brief Returns the dual part of this dual quaternion. DQ D() const; + /// @brief Returns the real part of this dual quaternion. DQ Re() const; + /// @brief Returns the imaginary part of this dual quaternion. DQ Im() const; + /** + * @brief Returns the conjugate of this dual quaternion. + * @see operator~(), transpose() + */ DQ conj() const; + /// @brief Returns the dual scalar corresponding to the norm of this dual quaternion. DQ norm() const; + /** + * @brief Returns the inverse of this dual quaternion, given by + * `conj()/(norm()^2)`. + * @see pinv() + */ DQ inv() const; + /** + * @brief Returns the translation quaternion of this unit dual + * quaternion, assuming `dq = r + DQ::E * 0.5 * p * r`, that is, the + * translation followed by rotation motion. + * @return The pure quaternion `p` representing the translation. + */ DQ translation() const; + /** + * @brief Returns the rotation quaternion of this unit dual quaternion. + * @throws std::runtime_error if this dual quaternion does not have unit norm. + */ DQ rotation() const; + /** + * @brief Returns the rotation axis of this unit dual quaternion. + * @return The pure quaternion representing the rotation axis + * (`nx*i + ny*j + nz*k`). + * @note If the rotation angle is zero, the axis is not well defined and, + * by convention, the axis `k` is returned. + * @throws std::runtime_error if this dual quaternion does not have unit norm. + */ DQ rotation_axis() const; + /** + * @brief Returns the rotation angle of this unit dual quaternion. + * @throws std::runtime_error if this dual quaternion does not have unit norm. + */ double rotation_angle() const; + /// @brief Returns the logarithm of this dual quaternion. DQ log() const; + /// @brief Returns the exponential of this pure dual quaternion. DQ exp() const; + /** + * @brief Returns this dual quaternion raised to the power of @p a. + * @param a The exponent. + */ DQ pow(const double a) const; + /** + * @brief Returns the unit dual quaternion corresponding to the + * translation part of this dual quaternion. More specifically, if + * `dq = r + DQ::E*0.5*p*r`, `tplus()` returns `1 + DQ::E*0.5*p`. + */ DQ tplus() const; + /// @brief Alias for tplus(). inline DQ T() const{return tplus();} + /// @brief Returns the Moore-Penrose pseudoinverse of this dual quaternion. DQ pinv() const; + /// @brief Returns the Hamilton operator H+ of this dual quaternion, restricted to its primary part. Matrix4d hamiplus4() const; + /// @brief Returns the Hamilton operator H- of this dual quaternion, restricted to its primary part. Matrix4d haminus4() const; + /// @brief Returns the Hamilton operator H+ of this dual quaternion. Matrix hamiplus8() const; + /// @brief Returns the Hamilton operator H- of this dual quaternion. Matrix haminus8() const; + /// @brief Maps the primary part of this dual quaternion into a 3-dimensional vector. Vector3d vec3() const; + /// @brief Maps the primary part of this dual quaternion into a 4-dimensional vector. Vector4d vec4() const; + /// @brief Maps this dual quaternion into a 6-dimensional vector, discarding the real part of both the primary and dual components. Matrix vec6() const; + /// @brief Maps this dual quaternion into an 8-dimensional vector. Matrix vec8() const; + /// @brief Returns the generalized Jacobian used in the mapping between the time derivative of a unit dual quaternion and the twist it represents. Matrix generalized_jacobian() const; + /// @brief Returns this dual quaternion normalized to unit norm. DQ normalize() const; + /** + * @brief Returns the sharp conjugate of this dual quaternion. + * @see operator~() + */ DQ sharp() const; + /** + * @brief Returns the adjoint transformation `this * dq2 * this'`. + * + * Given a Plücker line represented by the pure dual quaternion @p dq2, + * expressed with respect to the frame represented by this unit dual + * quaternion, `Ad()` returns the Plücker line expressed in the base frame. + * @param dq2 The pure dual quaternion representing a Plücker line. + * @see Adsharp() + */ DQ Ad(const DQ& dq2) const; + /** + * @brief Returns the sharp adjoint transformation `this.sharp() * dq2 * this'`. + * + * Given a plane represented by the dual quaternion @p dq2, expressed with + * respect to the frame represented by this unit dual quaternion, + * `Adsharp()` returns the plane expressed in the base frame. + * @param dq2 The dual quaternion representing a plane. + * @see Ad() + */ DQ Adsharp(const DQ& dq2) const; + /** + * @brief Given the unit quaternion represented by this dual quaternion, + * returns the partial derivative of `vec4()` with respect to + * `vec3(log())`. + * + * See Eq. (22) of Savino et al. (2020), "Pose consensus based on dual + * quaternion algebra with application to decentralized formation control + * of mobile manipulators." https://doi.org/10.1016/j.jfranklin.2019.09.045 + */ Matrix Q4() const; + /** + * @brief Given this unit dual quaternion, returns the partial derivative + * of `vec8()` with respect to `vec6(log())`. + * + * See Theorem 4 of Savino et al. (2020), "Pose consensus based on dual + * quaternion algebra with application to decentralized formation control + * of mobile manipulators." https://doi.org/10.1016/j.jfranklin.2019.09.045 + */ Matrix Q8() const; + /// @brief Returns a string representation of this dual quaternion. std::string to_string() const; //Operators + /// @brief Returns the additive inverse of this dual quaternion. DQ operator-() const; + /// @brief Returns true if this dual quaternion and @p dq2 are equal, up to a numerical threshold. bool operator==(const DQ& dq2) const; + /// @brief Returns true if this dual quaternion and @p dq2 are different, up to a numerical threshold. bool operator!=(const DQ& dq2) const; + /** + * @brief Casts this dual quaternion to a `double`. + * @throws std::runtime_error if this dual quaternion does not represent a real number. + */ explicit operator double() const; + /** + * @brief Casts this dual quaternion to an `int`. + * @throws std::runtime_error if this dual quaternion does not represent a real number. + */ explicit operator int() const; //Assigment operator template for scalars + /// @brief Assigns the scalar @p s to the real part of this dual quaternion, setting all remaining coefficients to zero. template ::value>> DQ& operator=(const Scalar& s) { @@ -171,122 +332,268 @@ class DQ{ };//DQ Class END //Operators +/// @brief Returns the primary part of @p dq. DQ P(const DQ& dq); +/// @brief Returns the dual part of @p dq. DQ D(const DQ& dq); +/// @brief Returns the real part of @p dq. DQ Re(const DQ& dq); +/// @brief Returns the imaginary part of @p dq. DQ Im(const DQ& dq); +/// @brief Returns the conjugate of @p dq. DQ conj(const DQ& dq); +/// @brief Returns the dual scalar corresponding to the norm of @p dq. DQ norm(const DQ& dq); +/** + * @brief Returns the inverse of @p dq, given by `conj(dq)/(norm(dq)^2)`. + * @see pinv(const DQ&) + */ DQ inv(const DQ& dq); +/** + * @brief Returns the translation quaternion of the unit dual quaternion @p dq, + * assuming `dq = r + DQ::E*0.5*p*r`. + */ DQ translation(const DQ& dq); +/** + * @brief Returns the rotation quaternion of the unit dual quaternion @p dq. + * @throws std::runtime_error if @p dq does not have unit norm. + */ DQ rotation(const DQ& dq); +/** + * @brief Returns the rotation axis (`nx*i + ny*j + nz*k`) of the unit dual + * quaternion @p dq. + * @note If the rotation angle is zero, the axis is not well defined and, + * by convention, the axis `k` is returned. + * @throws std::runtime_error if @p dq does not have unit norm. + */ DQ rotation_axis(const DQ& dq); +/** + * @brief Returns the rotation angle of the unit dual quaternion @p dq. + * @throws std::runtime_error if @p dq does not have unit norm. + */ double rotation_angle(const DQ& dq); +/// @brief Returns the logarithm of the dual quaternion @p dq. DQ log(const DQ& dq); +/// @brief Returns the exponential of the pure dual quaternion @p dq. DQ exp(const DQ& dq); +/** + * @brief Returns @p dq raised to the power of @p a. + * @param dq The dual quaternion base. + * @param a The exponent. + */ DQ pow(const DQ& dq, const double& a); +/** + * @brief Returns the unit dual quaternion corresponding to the translation + * part of @p dq. More specifically, if `dq = r + DQ::E*0.5*p*r`, `tplus()` + * returns `1 + DQ::E*0.5*p`. + */ DQ tplus(const DQ& dq); +/// @brief Alias for tplus(const DQ&). inline DQ T(const DQ& dq){return tplus(dq);} +/// @brief Returns the Moore-Penrose pseudoinverse of @p dq. DQ pinv(const DQ& dq); +/** + * @brief Returns the decompositional multiplication between @p dq1 and @p dq2. + * @param dq1 The first dual quaternion. + * @param dq2 The second dual quaternion. + */ DQ dec_mult(const DQ& dq1, const DQ& dq2); +/// @brief Returns the Hamilton operator H+ of @p dq, restricted to its primary part. Matrix4d hamiplus4(const DQ& dq); +/// @brief Returns the Hamilton operator H- of @p dq, restricted to its primary part. Matrix4d haminus4(const DQ& dq); +/// @brief Returns the Hamilton operator H+ of @p dq. Matrix hamiplus8(const DQ& dq); +/// @brief Returns the Hamilton operator H- of @p dq. Matrix haminus8(const DQ& dq); +/// @brief Returns the generalized Jacobian used in the mapping between the time derivative of the unit dual quaternion @p dq and the twist it represents. Matrix generalized_jacobian(const DQ& dq); +/// @brief Maps the primary part of @p dq into a 3-dimensional vector. Vector3d vec3(const DQ& dq); +/// @brief Maps the primary part of @p dq into a 4-dimensional vector. Vector4d vec4(const DQ& dq); +/// @brief Maps @p dq into a 6-dimensional vector, discarding the real part of both the primary and dual components. Matrix vec6(const DQ& dq); +/// @brief Maps @p dq into an 8-dimensional vector. Matrix vec8(const DQ& dq); +/** + * @brief Maps the pure quaternion @p dq into an expanded skew-symmetric matrix + * such that `vec4(cross(dq,v)) = crossmatrix4(dq)*vec4(v)`. + */ Matrix4d crossmatrix4(const DQ& dq); +/// @brief Returns @p dq normalized to unit norm. DQ normalize (const DQ& dq); +/** + * @brief Returns the sharp conjugate of @p dq. + */ DQ sharp(const DQ& dq); +/// @brief Returns the cross product between the pure dual quaternions @p dq1 and @p dq2. DQ cross(const DQ& dq1, const DQ& dq2); +/// @brief Returns the dot product between the pure dual quaternions @p dq1 and @p dq2. DQ dot(const DQ& dq1, const DQ& dq2); +/** + * @brief Returns the adjoint transformation `dq1 * dq2 * dq1'`. + * + * Given a Plücker line represented by the pure dual quaternion @p dq2, + * expressed with respect to the frame represented by the unit dual + * quaternion @p dq1, `Ad()` returns the Plücker line expressed in the base + * frame. + * @see Adsharp(const DQ&, const DQ&) + */ DQ Ad(const DQ& dq1, const DQ& dq2); +/** + * @brief Returns the sharp adjoint transformation `sharp(dq1) * dq2 * dq1'`. + * + * Given a plane represented by the dual quaternion @p dq2, expressed with + * respect to the frame represented by the unit dual quaternion @p dq1, + * `Adsharp()` returns the plane expressed in the base frame. + * @see Ad(const DQ&, const DQ&) + */ DQ Adsharp(const DQ& dq1, const DQ& dq2); +/** + * @brief Given the unit quaternion @p dq, returns the partial derivative of + * `vec4(dq)` with respect to `vec3(log(dq))`. + * + * See Eq. (22) of Savino et al. (2020), "Pose consensus based on dual + * quaternion algebra with application to decentralized formation control of + * mobile manipulators." https://doi.org/10.1016/j.jfranklin.2019.09.045 + */ Matrix Q4(const DQ& dq); +/** + * @brief Given the unit dual quaternion @p dq, returns the partial derivative + * of `vec8(dq)` with respect to `vec6(log(dq))`. + * + * See Theorem 4 of Savino et al. (2020), "Pose consensus based on dual + * quaternion algebra with application to decentralized formation control of + * mobile manipulators." https://doi.org/10.1016/j.jfranklin.2019.09.045 + */ Matrix Q8(const DQ& dq); +/** + * @brief Returns true if @p dq is a unit norm dual quaternion, false otherwise. + * @see is_pure(const DQ&), is_quaternion(const DQ&), is_real(const DQ&), is_real_number(const DQ&) + */ bool is_unit(const DQ& dq); +/** + * @brief Returns true if @p dq is pure (i.e., `Re(dq) = 0`), false otherwise. + * @see is_line(const DQ&), is_plane(const DQ&), is_pure_quaternion(const DQ&), is_quaternion(const DQ&), is_real(const DQ&), is_real_number(const DQ&), is_unit(const DQ&) + */ bool is_pure(const DQ& dq); +/** + * @brief Returns true if the imaginary part of @p dq is zero, false otherwise. + * @note A real dual quaternion is not necessarily a strict real number because + * it can also be a dual number. + * @see is_pure(const DQ&), is_quaternion(const DQ&), is_real_number(const DQ&), is_unit(const DQ&) + */ bool is_real(const DQ& dq); +/** + * @brief Returns true if both the dual and imaginary parts of @p dq are zero, false otherwise. + * @see is_pure(const DQ&), is_quaternion(const DQ&), is_real(const DQ&), is_unit(const DQ&) + */ bool is_real_number(const DQ& dq); +/** + * @brief Returns true if the dual part of @p dq is zero, false otherwise. + * @see is_pure(const DQ&), is_real(const DQ&), is_real_number(const DQ&), is_unit(const DQ&) + */ bool is_quaternion(const DQ& dq); +/** + * @brief Returns true if @p dq is a pure quaternion (i.e., `Re(dq) = D(dq) = 0`), false otherwise. + * @see is_quaternion(const DQ&), is_pure(const DQ&), is_real(const DQ&), is_real_number(const DQ&), is_unit(const DQ&) + */ bool is_pure_quaternion(const DQ& dq); +/** + * @brief Returns true if @p dq is a line (i.e., `Re(dq) = 0` and `norm(dq) = 1`), false otherwise. + * @see is_plane(const DQ&), is_pure(const DQ&), is_pure_quaternion(const DQ&), is_quaternion(const DQ&), is_real(const DQ&), is_real_number(const DQ&), is_unit(const DQ&) + */ bool is_line(const DQ& dq); +/** + * @brief Returns true if @p dq is a plane (i.e., it has unit norm and `Im(D(dq)) = 0`), false otherwise. + * @see is_pure(const DQ&), is_pure_quaternion(const DQ&), is_quaternion(const DQ&), is_real(const DQ&), is_real_number(const DQ&), is_unit(const DQ&) + */ bool is_plane(const DQ& dq); /************************************************************************* ************** DUAL QUATERNION CONSTANTS AND OPERATORS ****************** ************************************************************************/ +/// @brief Numerical threshold used to compare dual quaternions for equality. constexpr double DQ_threshold = 1e-12; +/// @brief Returns the dual quaternion addition `dq1 + dq2`. const DQ operator+(const DQ& dq1, const DQ& dq2) noexcept; +/// @brief Rvalue-reference overload of operator+(const DQ&, const DQ&) that reuses @p rdq1 to avoid a copy. const DQ operator+(DQ&& rdq1, const DQ& dq2) noexcept; +/// @brief Rvalue-reference overload of operator+(const DQ&, const DQ&) that reuses @p rdq2 to avoid a copy. const DQ operator+(const DQ& dq1, DQ&& rdq2) noexcept; +/// @brief Rvalue-reference overload of operator+(const DQ&, const DQ&) that reuses @p rdq1 to avoid a copy. const DQ operator+(DQ&& rdq1, DQ&& rdq2) noexcept; +/// @brief Returns the dual quaternion subtraction `dq1 - dq2`. const DQ operator-(const DQ& dq1, const DQ& dq2) noexcept; +/// @brief Rvalue-reference overload of operator-(const DQ&, const DQ&) that reuses @p rdq1 to avoid a copy. const DQ operator-(DQ&& rdq1, const DQ& dq2) noexcept; //const DQ operator-(const DQ& dq1, DQ&& rdq2) noexcept; //TODO: Think of a smart way to implement this +/// @brief Rvalue-reference overload of operator-(const DQ&, const DQ&) that reuses @p rdq1 to avoid a copy. const DQ operator-(DQ&& rdq1, DQ&& rdq2) noexcept; +/// @brief Returns the dual quaternion multiplication `dq1 * dq2`. const DQ operator*(const DQ& dq1, const DQ& dq2) noexcept; //Operator (<<) Overload +/// @brief Streams a string representation of @p dq into @p os. std::ostream& operator<<(std::ostream &os, const DQ& dq); //Constants +/// @brief Returns the conjugator matrix associated with vec8(). Matrix C8(); +/// @brief Returns the conjugator matrix associated with vec4(). Matrix C4(); -const DQ E_ = DQ(0,0,0,0,1,0,0,0); -const DQ i_ = DQ(0,1,0,0,0,0,0,0); -const DQ j_ = DQ(0,0,1,0,0,0,0,0); -const DQ k_ = DQ(0,0,0,1,0,0,0,0); +const DQ E_ = DQ(0,0,0,0,1,0,0,0); ///< Shortcut for the dual unit DQ::E. +const DQ i_ = DQ(0,1,0,0,0,0,0,0); ///< Shortcut for the imaginary unit DQ::i. +const DQ j_ = DQ(0,0,1,0,0,0,0,0); ///< Shortcut for the imaginary unit DQ::j. +const DQ k_ = DQ(0,0,0,1,0,0,0,0); ///< Shortcut for the imaginary unit DQ::k. /************************************************************************* ************** DUAL QUATERNIONS AND SCALAR OPERATOR TEMPLATES *********** diff --git a/include/dqrobotics/internal/_dq_linesegment.h b/include/dqrobotics/internal/_dq_linesegment.h index 9c30789..5b1b79d 100644 --- a/include/dqrobotics/internal/_dq_linesegment.h +++ b/include/dqrobotics/internal/_dq_linesegment.h @@ -31,32 +31,112 @@ namespace DQ_robotics namespace internal { +/** + * @brief Internal routines for closest-feature queries between line segments. + * + * This class supports the line-segment distance algorithms used inside the C++ + * implementation of DQ Robotics. It is declared in the internal namespace and + * is not part of the stable public API. + * + * @internal + */ class LineSegment { public: + /** + * @brief Identifies which primitive of a line segment participates in a closest pair. + */ enum class Element{ - Line,P1,P2 + /** @brief The supporting infinite line of the segment. */ + Line, + /** @brief The first endpoint of the segment. */ + P1, + /** @brief The second endpoint of the segment. */ + P2 }; + /** + * @brief Groups the supporting line and the two endpoints of a line segment. + */ using Primitives = std::tuple; + + /** + * @brief Stores the closest primitive selected from each of two line segments. + */ using ClosestElements = std::tuple; + + /** + * @brief Stores a closest-element pair together with its squared distance. + */ using ClosestElementsAndDistance = std::tuple; + /** + * @brief Determines the closest primitives between two line segments. + * + * The algorithm compares admissible line-line, line-endpoint, and + * endpoint-endpoint candidates, discarding infeasible line-to-point cases, + * and returns the valid pair with minimum squared distance. + * + * @param line_1_primitives A tuple `(line, point_1, point_2)` describing the first segment. + * @param line_2_primitives A tuple `(line, point_1, point_2)` describing the second segment. + * @return A tuple containing the closest primitive pair and the corresponding squared distance. + */ static ClosestElementsAndDistance closest_elements_between_line_segments(const Primitives& line_1_primitives, const Primitives& line_2_primitives); + /** + * @brief Tests whether a point lies strictly inside a line segment. + * + * This check compares the squared distances from the point to each endpoint + * against the segment squared length. It assumes the point already belongs to + * the supporting line; the collinearity test is not performed here. + * + * @param point The point to be tested. + * @param line_1_primitives A tuple `(line, point_1, point_2)` describing the segment. + * @return `true` if the point lies strictly between the endpoints and `false` otherwise. + * @note Endpoints are considered outside because strict inequalities are used. + */ static bool is_inside_line_segment(const DQ &point, const Primitives &line_1_primitives); + /** + * @brief Converts an Element enumerator to its string representation. + * + * @param e The enumerator to be converted. + * @return The string representation of `e`. + * @throws std::runtime_error If `e` does not match a known enumerator. + */ static std::string to_string(const Element& e); private: + /** + * @brief Selects the best valid closest-pair candidate seen so far. + * + * Invalid candidates are represented with `NaN` distances and are ignored. + * When both candidates are valid, the one with smaller squared distance is + * returned. + * + * @param current The current best result. + * @param candidate The new candidate to be compared against `current`. + * @return The preferred result after the comparison. + */ static ClosestElementsAndDistance _update_closest_pair( const ClosestElementsAndDistance& current, const ClosestElementsAndDistance& candidate); + /** + * @brief Evaluates a line-to-point candidate for the line-segment search. + * + * The method projects the point onto the supporting line of the segment. If + * the projection lies inside the segment, it returns the squared point-to-line + * distance; otherwise, it returns `NaN` to mark the candidate as infeasible. + * + * @param line_segment A tuple `(line, point_1, point_2)` describing the segment. + * @param point The point from the other segment. + * @return The squared point-to-line distance for a feasible candidate, or `NaN` otherwise. + */ static double _line_to_point_feasibility_and_distance( const Primitives& line_segment, const DQ& point); diff --git a/include/dqrobotics/robot_control/DQ_ClassicQPController.h b/include/dqrobotics/robot_control/DQ_ClassicQPController.h index b6b84e3..31f86a5 100644 --- a/include/dqrobotics/robot_control/DQ_ClassicQPController.h +++ b/include/dqrobotics/robot_control/DQ_ClassicQPController.h @@ -28,17 +28,53 @@ This file is part of DQ Robotics. namespace DQ_robotics { +/** + * @brief Implements the classic quadratic-programming kinematic controller based on task-space variables. + * + * This controller uses a quadratic objective built from the task Jacobian and the + * Euclidean task-space error. Its default isotropic damping is initialized to 1e-3. + * + * @see DQ_QuadraticProgrammingController, DQ_PseudoinverseController + */ class DQ_ClassicQPController:public DQ_QuadraticProgrammingController { public: DQ_ClassicQPController() = delete; //Deprecated + /** + * @brief Constructs a classic QP controller from legacy raw pointers. + * + * @param robot Non-owning pointer to the robot kinematic model. + * @param solver Non-owning pointer to the quadratic-programming solver. + */ DQ_ClassicQPController(DQ_Kinematics* robot, DQ_QuadraticProgrammingSolver* solver); + /** + * @brief Constructs a classic QP controller from shared pointers. + * + * @param robot Shared pointer to the robot kinematic model. + * @param solver Shared pointer to the quadratic-programming solver. + */ DQ_ClassicQPController(const std::shared_ptr& robot, const std::shared_ptr& solver); + /** + * @brief Computes the symmetric matrix H used in the quadratic objective. + * + * The returned matrix corresponds to J'J plus isotropic damping. The second + * argument of the abstract interface is unused by this controller. + * + * @param J Task Jacobian associated with the current control objective. + * @return The symmetric matrix H of the quadratic objective. + */ MatrixXd compute_objective_function_symmetric_matrix(const MatrixXd& J, const VectorXd&) override; + /** + * @brief Computes the linear vector f used in the quadratic objective. + * + * @param J Task Jacobian associated with the current control objective. + * @param task_error Current task-space error. + * @return The linear component f of the quadratic objective. + */ VectorXd compute_objective_function_linear_component(const MatrixXd& J, const VectorXd& task_error) override; }; diff --git a/include/dqrobotics/robot_control/DQ_KinematicConstrainedController.h b/include/dqrobotics/robot_control/DQ_KinematicConstrainedController.h index f265cfa..36d4856 100644 --- a/include/dqrobotics/robot_control/DQ_KinematicConstrainedController.h +++ b/include/dqrobotics/robot_control/DQ_KinematicConstrainedController.h @@ -32,22 +32,57 @@ using namespace Eigen; namespace DQ_robotics { +/** + * @brief Abstract superclass used to define concrete kinematic controllers with algebraic constraints. + * + * This class extends DQ_KinematicController with equality and inequality constraints + * on the control input. Constrained controllers can store matrices and vectors that + * are later supplied to optimization-based control laws. + * + * @see DQ_KinematicController, DQ_QuadraticProgrammingController + */ class DQ_KinematicConstrainedController: public DQ_KinematicController { protected: + /** @brief Matrix used in equality constraints of the form Aeq*u = beq. */ MatrixXd equality_constraint_matrix_; + /** @brief Vector used in equality constraints of the form Aeq*u = beq. */ VectorXd equality_constraint_vector_; + /** @brief Matrix used in inequality constraints of the form A*u <= b. */ MatrixXd inequality_constraint_matrix_; + /** @brief Vector used in inequality constraints of the form A*u <= b. */ VectorXd inequality_constraint_vector_; + /** + * @brief Constructs a constrained controller from a legacy raw robot pointer. + * + * @param robot Non-owning pointer to the robot kinematic model. + */ [[deprecated("Use the smart pointer version instead")]] DQ_KinematicConstrainedController(DQ_Kinematics* robot); + /** + * @brief Constructs a constrained controller from a shared robot pointer. + * + * @param robot Shared pointer to the robot kinematic model. + */ DQ_KinematicConstrainedController(const std::shared_ptr& robot); public: //Remove default constructor DQ_KinematicConstrainedController()=delete; + /** + * @brief Sets the equality constraint passed to constrained control laws. + * + * @param B Equality-constraint matrix. + * @param b Equality-constraint vector. + */ virtual void set_equality_constraint(const MatrixXd& B, const VectorXd& b); + /** + * @brief Sets the inequality constraint passed to constrained control laws. + * + * @param B Inequality-constraint matrix. + * @param b Inequality-constraint vector. + */ virtual void set_inequality_constraint(const MatrixXd& B, const VectorXd& b); }; diff --git a/include/dqrobotics/robot_control/DQ_KinematicController.h b/include/dqrobotics/robot_control/DQ_KinematicController.h index da32c42..8a82cc0 100644 --- a/include/dqrobotics/robot_control/DQ_KinematicController.h +++ b/include/dqrobotics/robot_control/DQ_KinematicController.h @@ -29,85 +29,282 @@ This file is part of DQ Robotics. namespace DQ_robotics { +/** + * @brief Enumerates the task-space objectives supported by DQ_KinematicController. + * + * @see DQ_KinematicController + */ enum ControlObjective { + /** @brief No control objective has been selected yet. */ None, + /** @brief Control the squared distance between the end-effector translation and the origin. */ Distance, + /** @brief Control the signed distance from the end-effector point to a target plane. */ DistanceToPlane, + /** @brief Control a line primitive attached to the end-effector. */ Line, + /** @brief Control a plane primitive attached to the end-effector. */ Plane, + /** @brief Control the full end-effector pose. */ Pose, + /** @brief Control only the end-effector orientation. */ Rotation, + /** @brief Control only the end-effector translation. */ Translation }; +/** + * @brief Abstract class that defines an interface to implement kinematic controllers for robots described by DQ_Kinematics. + * + * The controller stores a task-space control objective together with the primitives, + * gains, damping terms, and stability-monitoring variables required by concrete + * kinematic control laws. Subclasses are responsible for implementing the mapping + * from task errors to reference joint velocities through either pure pseudoinverse, + * constrained, or optimization-based formulations. + * + * @note This class remains abstract because subclasses must implement the setpoint + * and tracking control laws. + * @see DQ_PseudoinverseController, DQ_KinematicConstrainedController, DQ_ClassicQPController + */ class DQ_KinematicController { protected: //Deprecated together with the raw pointer constructors but without the C++14 attribute as it is too noisy. + /** @brief Legacy non-owning pointer to the robot model kept for backwards compatibility. */ DQ_Kinematics* robot_; + /** @brief Shared pointer to the robot model used by the smart-pointer constructors. */ std::shared_ptr robot_sptr_; + /** @brief Current task-space objective handled by the controller. */ ControlObjective control_objective_; + /** @brief Primitive rigidly attached to the end-effector for line or plane control tasks. */ DQ attached_primitive_; + /** @brief Target primitive used by objectives that require convergence to a workspace primitive. */ DQ target_primitive_; + /** @brief Proportional gain used in the control law. */ double gain_; + /** @brief Isotropic damping used to regularize singular or ill-conditioned Jacobians. */ double damping_; + /** @brief True when the error evolution indicates convergence to a stable region. */ bool system_reached_stable_region_; + /** @brief Last joint-velocity control signal computed by the controller. */ VectorXd last_control_signal_; + /** @brief Last task-space error stored for stability verification. */ VectorXd last_error_signal_; + /** @brief Threshold on the variation of the task error used to detect a stable region. */ double stability_threshold_; + /** @brief Counter of consecutive iterations whose error variation is below the stability threshold. */ int stability_counter_; + /** @brief Number of consecutive stable iterations required to flag convergence to a stable region. */ int stability_counter_max_; //For backwards compatibility reasons, to be removed + /** + * @brief Returns the active robot pointer regardless of the constructor used. + * + * @return Pointer to the associated robot model. + */ DQ_Kinematics* _get_robot_ptr() const; + /** + * @brief Returns the stored shared pointer to the robot model. + * + * @return Shared pointer to the associated robot model. + * @throws std::runtime_error If the controller was not constructed with a shared pointer. + */ std::shared_ptr _get_robot() const; //Deprecated + /** + * @brief Constructs a controller from a legacy raw robot pointer. + * + * @param robot Non-owning pointer to the robot kinematic model. + */ [[deprecated("Use the smart pointer version instead.")]] DQ_KinematicController(DQ_Kinematics* robot); + /** + * @brief Constructs a controller from a shared robot pointer. + * + * @param robot Shared pointer to the robot kinematic model. + */ DQ_KinematicController(const std::shared_ptr& robot); + /** + * @brief Constructs a controller with default internal state. + * + * Concrete subclasses use this constructor to initialize gains, damping, + * task selection, and stability-monitoring variables before binding a robot. + */ DQ_KinematicController(); -public: +public: + /** + * @brief Returns the current control objective. + * + * @return The configured control objective. + */ ControlObjective get_control_objective() const; + /** + * @brief Returns the task Jacobian associated with the current control objective. + * + * The returned matrix depends on the selected objective and can correspond to + * pose, rotation, translation, distance, point-to-plane distance, line, or + * plane kinematics. + * + * @param q Vector containing the current joint configurations of the robot. + * @return The Jacobian associated with the current task variable. + * @throws std::runtime_error If the number of joints is incompatible with the robot model, + * if the control objective was not set, if a plane target was required but not configured, + * or if the control objective is unknown. + */ MatrixXd get_jacobian(const VectorXd& q) const; + /** + * @brief Returns the current task variable associated with the control objective. + * + * The returned vector contains the task-space quantity regulated by the controller, + * such as pose coordinates, translation, rotation, distance, line coordinates, + * or plane coordinates. + * + * @param q Vector containing the current joint configurations of the robot. + * @return The task variable corresponding to the configured objective. + * @throws std::runtime_error If the number of joints is incompatible with the robot model, + * if the control objective was not set, if a plane target was required but not configured, + * or if the control objective is unknown. + */ VectorXd get_task_variable(const VectorXd& q) const; + /** + * @brief Returns the last task-space error signal computed by the controller. + * + * @return The last stored task-space error. + */ VectorXd get_last_error_signal() const; + /** + * @brief Verifies whether a control objective has been selected. + * + * @return True if the control objective is different from ControlObjective::None, and false otherwise. + */ bool is_set() const; + /** + * @brief Indicates whether the closed-loop system has reached a stable region. + * + * @return True if the error variation stayed below the stability threshold for the required number of iterations. + */ bool system_reached_stable_region() const; + /** + * @brief Sets the control objective. + * + * This method also resizes the internally stored error vector so that it matches + * the dimension of the selected task variable. + * + * @param control_objective The desired control objective. + */ void set_control_objective(const ControlObjective& control_objective); + /** + * @brief Sets the controller gain. + * + * @param gain The proportional gain used in the control law. + */ void set_gain(const double& gain); + /** + * @brief Returns the controller gain. + * + * @return The current proportional gain. + */ double get_gain() const; + /** + * @brief Sets the isotropic damping used by singularity-robust controllers. + * + * @param damping The damping coefficient. + */ void set_damping(const double& damping); + /** + * @brief Returns the isotropic damping coefficient. + * + * @return The current damping coefficient. + */ double get_damping() const; + /** + * @brief Sets the threshold used to detect convergence to a stable region. + * + * @param threshold Maximum norm variation between consecutive task errors that is considered stable. + */ void set_stability_threshold(const double& threshold); + /** + * @brief Attaches a primitive to the end-effector for primitive-based objectives. + * + * For example, the primitive can represent a line or plane rigidly attached to the + * end-effector and later used by ControlObjective::Line or ControlObjective::Plane. + * + * @param primitive Dual quaternion representation of the attached primitive. + */ void set_primitive_to_effector(const DQ& primitive); + /** + * @brief Sets the target primitive for primitive-based convergence tasks. + * + * @param primitive Dual quaternion representation of the target primitive. + */ void set_target_primitive(const DQ& primitive); + /** + * @brief Sets the number of consecutive stable iterations required to declare convergence. + * + * @param max Maximum value of the stability counter. + */ void set_stability_counter_max(const int& max); + /** + * @brief Resets the stability counter and clears the stable-region flag. + */ void reset_stability_counter(); //Virtual + /** + * @brief Virtual destructor. + */ virtual ~DQ_KinematicController()=default; + /** + * @brief Computes the reference joint velocities for a setpoint task. + * + * Pure virtual interface contract implemented by concrete kinematic controllers. + * + * @param q Vector containing the current joint configurations of the robot. + * @param task_reference Vector containing the desired value for the chosen control task. + * @return The reference joint velocities. + */ virtual VectorXd compute_setpoint_control_signal(const VectorXd& q, const VectorXd& task_reference)=0; + /** + * @brief Computes the reference joint velocities for a tracking task with feedforward. + * + * Pure virtual interface contract implemented by concrete kinematic controllers. + * + * @param q Vector containing the current joint configurations of the robot. + * @param task_reference Vector containing the desired value for the chosen control task. + * @param feed_forward Time derivative of the task reference expressed in task space. + * @return The reference joint velocities. + */ virtual VectorXd compute_tracking_control_signal(const VectorXd& q, const VectorXd& task_reference, const VectorXd& feed_forward)=0; + /** + * @brief Updates the internal stability indicators using the current task error. + * + * The system is considered to have reached a stable region when the variation of + * the task error remains below the configured threshold for a sufficient number of + * consecutive iterations. + * + * @param task_error Current task-space error. + */ virtual void verify_stability(const VectorXd& task_error); }; diff --git a/include/dqrobotics/robot_control/DQ_NumericalFilteredPseudoInverseController.h b/include/dqrobotics/robot_control/DQ_NumericalFilteredPseudoInverseController.h index 4d4da44..bb8acd3 100644 --- a/include/dqrobotics/robot_control/DQ_NumericalFilteredPseudoInverseController.h +++ b/include/dqrobotics/robot_control/DQ_NumericalFilteredPseudoInverseController.h @@ -32,33 +32,114 @@ This file is part of DQ Robotics. namespace DQ_robotics { +/** + * @brief Implements a singularity-robust pseudoinverse controller with numerical filtered damping. + * + * This controller extends DQ_PseudoinverseController with the numerical filtering strategy + * described by Chiaverini for singularity-robust kinematic control. The Jacobian singular-value + * decomposition is used to build additional damping only along singular directions whose singular + * values fall inside the configured singular region. When the singular region size or the maximum + * numerical filtered damping is zero, when the Jacobian is full rank, or when the filtered damping + * evaluates to zero, the controller reduces to DQ_PseudoinverseController. + * + * @see DQ_PseudoinverseController, DQ_KinematicController + */ class DQ_NumericalFilteredPseudoinverseController: public DQ_PseudoinverseController { protected: + /** @brief Size of the singular region used to activate numerical filtered damping. */ double epsilon_; //Size of the singular region, described on the text after Eq. (15) + /** @brief Maximum numerical filtered damping applied inside the singular region. */ double lambda_max_; //Maximum value for the numerical filtered damping, described on the text after Eq. (15) //double damping_; //(Member variable of DQ_KinematicController: Isotropic damping described on the text above Eq. (20) //log + /** @brief Last filtered damping matrix computed from the Jacobian singular vectors. */ MatrixXd last_filtered_damping_; + /** @brief Rank of the last Jacobian processed by the controller. */ double last_jacobian_rank_; + /** @brief Singular value decomposition of the last Jacobian, stored as (U, S, V). */ std::tuple last_jacobian_svd_; public: DQ_NumericalFilteredPseudoinverseController() = delete; + /** + * @brief Constructs a controller from a legacy raw robot pointer. + * + * @param robot Non-owning pointer to the robot kinematic model. + */ [[deprecated("Use the smart pointer version instead")]] DQ_NumericalFilteredPseudoinverseController(DQ_Kinematics* robot); + /** + * @brief Constructs a controller from a shared robot pointer. + * + * @param robot Shared pointer to the robot kinematic model. + */ DQ_NumericalFilteredPseudoinverseController(const std::shared_ptr& robot); + /** + * @brief Computes the reference joint velocities for a setpoint task using numerical filtered damping. + * + * @param q Vector containing the current joint configurations of the robot. + * @param task_reference Vector containing the desired value for the chosen control task. + * @return The reference joint velocities. + * @throws std::runtime_error If the controller was not configured with a valid control objective. + */ VectorXd compute_setpoint_control_signal(const VectorXd& q, const VectorXd& task_reference) override; + /** + * @brief Computes the reference joint velocities for a tracking task using numerical filtered damping. + * + * @param q Vector containing the current joint configurations of the robot. + * @param task_reference Vector containing the desired value for the chosen control task. + * @param feed_forward Time derivative of the task reference expressed in task space. + * @return The reference joint velocities. + * @throws std::runtime_error If the controller was not configured with a valid control objective. + */ VectorXd compute_tracking_control_signal(const VectorXd& q, const VectorXd& task_reference, const VectorXd& feed_forward) override; + /** + * @brief Sets the maximum numerical filtered damping. + * + * @param numerical_filtered_damping Maximum damping value applied inside the singular region. + */ void set_maximum_numerical_filtered_damping(const double& numerical_filtered_damping); + /** + * @brief Sets the size of the singular region. + * + * @param singular_region_size Singular-value threshold that defines the numerical filtering region. + * @throws std::range_error If @p singular_region_size is negative. + */ void set_singular_region_size(const double& singular_region_size); + /** + * @brief Returns the maximum numerical filtered damping. + * + * @return The configured maximum numerical filtered damping. + */ double get_maximum_numerical_filtered_damping() const; + /** + * @brief Returns the size of the singular region. + * + * @return The configured singular region size. + */ double get_singular_region_size() const; + /** + * @brief Returns the filtered damping matrix computed in the last control step. + * + * @return The last filtered damping matrix. + */ MatrixXd get_last_filtered_damping() const; + /** + * @brief Returns the rank of the last processed Jacobian. + * + * @return The last Jacobian rank. + * @note The returned value is initialized with -1 before any control signal is computed. + */ int get_last_jacobian_rank() const; + /** + * @brief Returns the singular value decomposition of the last processed Jacobian. + * + * @return A tuple containing the matrices (U, S, V) from the last Jacobian SVD. + */ std::tuple get_last_jacobian_svd() const; }; diff --git a/include/dqrobotics/robot_control/DQ_PseudoinverseController.h b/include/dqrobotics/robot_control/DQ_PseudoinverseController.h index 68349fa..37cda2d 100644 --- a/include/dqrobotics/robot_control/DQ_PseudoinverseController.h +++ b/include/dqrobotics/robot_control/DQ_PseudoinverseController.h @@ -26,16 +26,52 @@ This file is part of DQ Robotics. namespace DQ_robotics { +/** + * @brief Implements a kinematic control law based on the Jacobian pseudoinverse and an Euclidean task-space error. + * + * This controller computes reference joint velocities from the task-space error using + * the Moore-Penrose pseudoinverse when the damping is zero, or a damped least-squares + * inverse when isotropic damping is enabled. + * + * @see DQ_KinematicController, DQ_NumericalFilteredPseudoinverseController + */ class DQ_PseudoinverseController: public DQ_KinematicController { public: DQ_PseudoinverseController() = delete; + /** + * @brief Constructs a controller from a legacy raw robot pointer. + * + * @param robot Non-owning pointer to the robot kinematic model. + */ [[deprecated("Use the smart pointer version instead")]] DQ_PseudoinverseController(DQ_Kinematics* robot); + /** + * @brief Constructs a controller from a shared robot pointer. + * + * @param robot Shared pointer to the robot kinematic model. + */ DQ_PseudoinverseController(const std::shared_ptr& robot); + /** + * @brief Computes the reference joint velocities that drive the task-space error to zero. + * + * @param q Vector containing the current joint configurations of the robot. + * @param task_reference Vector containing the desired value for the chosen control task. + * @return The reference joint velocities. + * @throws std::runtime_error If the controller was not configured with a valid control objective. + */ VectorXd compute_setpoint_control_signal(const VectorXd& q, const VectorXd& task_reference) override; + /** + * @brief Computes the reference joint velocities for a time-varying task-space reference. + * + * @param q Vector containing the current joint configurations of the robot. + * @param task_reference Vector containing the desired value for the chosen control task. + * @param feed_forward Time derivative of the task reference expressed in task space. + * @return The reference joint velocities. + * @throws std::runtime_error If the controller was not configured with a valid control objective. + */ VectorXd compute_tracking_control_signal(const VectorXd& q, const VectorXd& task_reference, const VectorXd& feed_forward) override; }; diff --git a/include/dqrobotics/robot_control/DQ_QuadraticProgrammingController.h b/include/dqrobotics/robot_control/DQ_QuadraticProgrammingController.h index 83f329f..0590a4f 100644 --- a/include/dqrobotics/robot_control/DQ_QuadraticProgrammingController.h +++ b/include/dqrobotics/robot_control/DQ_QuadraticProgrammingController.h @@ -28,27 +28,96 @@ using namespace Eigen; namespace DQ_robotics { +/** + * @brief Abstract class that defines task-space kinematic controllers based on quadratic programming. + * + * Although many kinematic controllers can be written as quadratic programs, this class + * targets formulations whose objective function depends on task-space quantities such as + * the robot Jacobian and the task-space error. Subclasses define the symmetric and linear + * terms of the quadratic objective, while this class assembles the constrained optimization + * problem and delegates it to a DQ_QuadraticProgrammingSolver. + * + * @note This class remains abstract because subclasses must implement the objective-function terms. + * @see DQ_KinematicConstrainedController, DQ_ClassicQPController, DQ_QuadraticProgrammingSolver + */ class DQ_QuadraticProgrammingController:public DQ_KinematicConstrainedController { protected: //Deprecated together with the raw pointer constructors but without the C++14 attribute as it is too noisy. + /** @brief Legacy non-owning pointer to the quadratic-programming solver kept for backwards compatibility. */ DQ_QuadraticProgrammingSolver* qp_solver_; + /** @brief Shared pointer to the quadratic-programming solver used by the smart-pointer constructors. */ std::shared_ptr qp_solver_sptr_; + /** + * @brief Returns the active quadratic-programming solver pointer. + * + * @return Pointer to the solver used to solve the control problem. + */ DQ_QuadraticProgrammingSolver* _get_solver_ptr(); + /** + * @brief Constructs a controller from legacy raw pointers. + * + * @param robot Non-owning pointer to the robot kinematic model. + * @param solver Non-owning pointer to the quadratic-programming solver. + */ [[deprecated("Use the smart pointer version instead")]] DQ_QuadraticProgrammingController(DQ_Kinematics *robot, DQ_QuadraticProgrammingSolver *solver); + /** + * @brief Constructs a controller from shared pointers. + * + * @param robot Shared pointer to the robot kinematic model. + * @param solver Shared pointer to the quadratic-programming solver. + */ DQ_QuadraticProgrammingController(const std::shared_ptr& robot, const std::shared_ptr& solver); public: //Remove default constructor DQ_QuadraticProgrammingController()=delete; + /** + * @brief Computes the symmetric matrix H of the quadratic objective. + * + * Pure virtual interface contract implemented by concrete quadratic-programming controllers. + * + * @param J Task Jacobian associated with the current control objective. + * @param task_error Task-space error, possibly combined with feedforward terms by the caller. + * @return The symmetric matrix H in the quadratic objective. + */ virtual MatrixXd compute_objective_function_symmetric_matrix(const MatrixXd& J, const VectorXd& task_error)=0; + /** + * @brief Computes the linear vector f of the quadratic objective. + * + * Pure virtual interface contract implemented by concrete quadratic-programming controllers. + * + * @param J Task Jacobian associated with the current control objective. + * @param task_error Task-space error, possibly combined with feedforward terms by the caller. + * @return The linear component f in the quadratic objective. + */ virtual VectorXd compute_objective_function_linear_component(const MatrixXd& J, const VectorXd& task_error)=0; + /** + * @brief Computes the reference joint velocities for a setpoint task. + * + * @param q Vector containing the current joint configurations of the robot. + * @param task_reference Vector containing the desired value for the chosen control task. + * @return The reference joint velocities obtained from the quadratic program. + * @throws std::runtime_error If the controller is unset or if incompatible task, Jacobian, or feedforward sizes are detected. + */ virtual VectorXd compute_setpoint_control_signal(const VectorXd&q, const VectorXd& task_reference) override; + /** + * @brief Computes the reference joint velocities for a tracking task with feedforward. + * + * The optimization problem uses the stored equality and inequality constraints together + * with the objective-function terms provided by the concrete subclass. + * + * @param q Vector containing the current joint configurations of the robot. + * @param task_reference Vector containing the desired value for the chosen control task. + * @param feed_forward Time derivative of the task reference expressed in task space. + * @return The reference joint velocities obtained from the quadratic program. + * @throws std::runtime_error If the controller is unset or if incompatible task, Jacobian, or feedforward sizes are detected. + */ virtual VectorXd compute_tracking_control_signal(const VectorXd&q, const VectorXd& task_reference, const VectorXd& feed_forward) override; }; diff --git a/include/dqrobotics/robot_modeling/DQ_CooperativeDualTaskSpace.h b/include/dqrobotics/robot_modeling/DQ_CooperativeDualTaskSpace.h index 168f5fe..2762ea8 100644 --- a/include/dqrobotics/robot_modeling/DQ_CooperativeDualTaskSpace.h +++ b/include/dqrobotics/robot_modeling/DQ_CooperativeDualTaskSpace.h @@ -29,28 +29,101 @@ This file is part of DQ Robotics. namespace DQ_robotics { +/** + * @brief Implements the cooperative dual task-space formulation for two robots. + * + * The cooperative variables are the absolute pose and the relative pose of the + * two end effectors, together with their corresponding Jacobians. The combined + * configuration vector is assumed to be theta = [q1; q2], where q1 and q2 are + * the configuration vectors of the first and second robots. + * + * @see DQ_Kinematics + */ class DQ_CooperativeDualTaskSpace { private: + /** @brief Pointer to the first robot model. */ DQ_Kinematics* robot1_; + /** @brief Pointer to the second robot model. */ DQ_Kinematics* robot2_; public: //Remove default constructor + /** @brief Deleted default constructor. */ DQ_CooperativeDualTaskSpace()=delete; + /** + * @brief Constructs a cooperative dual task-space system from two robot models. + * + * The object does not take ownership of the provided pointers. + * + * @param robot1 Pointer to the first robot model. + * @param robot2 Pointer to the second robot model. + */ DQ_CooperativeDualTaskSpace(DQ_Kinematics* robot1, DQ_Kinematics* robot2); + /** + * @brief Returns the pose of the first end effector. + * + * @param theta Combined configuration vector [q1; q2]. + * @return Pose of the first end effector. + */ DQ pose1(const VectorXd& theta); + /** + * @brief Returns the pose of the second end effector. + * + * @param theta Combined configuration vector [q1; q2]. + * @return Pose of the second end effector. + */ DQ pose2(const VectorXd& theta); + /** + * @brief Returns the pose Jacobian of the first robot. + * + * @param theta Combined configuration vector [q1; q2]. + * @return Pose Jacobian of the first robot end effector. + */ MatrixXd pose_jacobian1(const VectorXd& theta); + /** + * @brief Returns the pose Jacobian of the second robot. + * + * @param theta Combined configuration vector [q1; q2]. + * @return Pose Jacobian of the second robot end effector. + */ MatrixXd pose_jacobian2(const VectorXd& theta); + /** + * @brief Computes the relative pose between the two end effectors. + * + * The returned dual quaternion maps the second end-effector frame to the first one. + * + * @param theta Combined configuration vector [q1; q2]. + * @return Relative pose between the two end effectors. + */ DQ relative_pose(const VectorXd& theta); + /** + * @brief Computes the absolute pose of the cooperative system. + * + * The absolute pose corresponds to a frame located midway between the two end effectors. + * + * @param theta Combined configuration vector [q1; q2]. + * @return Absolute pose of the cooperative system. + */ DQ absolute_pose(const VectorXd& theta); + /** + * @brief Computes the Jacobian of the relative pose. + * + * @param theta Combined configuration vector [q1; q2]. + * @return Relative-pose Jacobian. + */ MatrixXd relative_pose_jacobian(const VectorXd& theta); + /** + * @brief Computes the Jacobian of the absolute pose. + * + * @param theta Combined configuration vector [q1; q2]. + * @return Absolute-pose Jacobian. + */ MatrixXd absolute_pose_jacobian(const VectorXd& theta); }; diff --git a/include/dqrobotics/robot_modeling/DQ_DifferentialDriveRobot.h b/include/dqrobotics/robot_modeling/DQ_DifferentialDriveRobot.h index 2d15c16..05c36cb 100644 --- a/include/dqrobotics/robot_modeling/DQ_DifferentialDriveRobot.h +++ b/include/dqrobotics/robot_modeling/DQ_DifferentialDriveRobot.h @@ -29,21 +29,89 @@ This file is part of DQ Robotics. namespace DQ_robotics { +/** + * @brief Basic implementation of a differential-drive mobile robot. + * + * The robot pose is modeled as a holonomic base with configuration + * q = [x, y, phi]^T, while the actuation is described by the angular + * velocities of the right and left wheels. The pose Jacobians in this class + * already account for the nonholonomic rolling constraint. + * + * @see DQ_HolonomicBase + */ class DQ_DifferentialDriveRobot : public DQ_HolonomicBase { protected: + /** @brief Radius of each wheel, in meters. */ double wheel_radius_; + /** @brief Distance between the wheels, in meters. */ double distance_between_wheels_; public: + /** + * @brief Constructs a differential-drive robot. + * + * @param wheel_radius Radius of each wheel, in meters. + * @param distance_between_wheels Distance between the wheels, in meters. + */ DQ_DifferentialDriveRobot(const double& wheel_radius, const double& distance_between_wheels); + /** + * @brief Computes the constraint Jacobian relating wheel velocities to configuration velocities. + * + * The returned matrix satisfies [x_dot, y_dot, phi_dot]^T = J * [wr, wl]^T, + * where wr and wl are the right- and left-wheel angular velocities. + * + * @param phi Planar orientation of the robot. + * @return The differential-drive constraint Jacobian. + */ MatrixXd constraint_jacobian(const double& phi) const; + /** + * @brief Computes the time derivative of the differential-drive constraint Jacobian. + * + * @param phi Planar orientation of the robot. + * @param phi_dot Time derivative of @p phi. + * @return The time derivative of the differential-drive constraint Jacobian. + */ MatrixXd constraint_jacobian_derivative(const double& phi, const double& phi_dot) const; + /** + * @brief Computes the constrained pose Jacobian. + * + * The returned Jacobian maps wheel angular velocities to the time derivative + * of the base pose. Because there are two independent wheel velocities, the + * valid values of @p to_link are 0 and 1. + * + * @param q Configuration vector [x, y, phi]^T. + * @param to_link Column index of the partial Jacobian to be returned. + * @return The constrained pose Jacobian up to the requested column. + * @throws std::runtime_error If @p to_link is not 0 or 1. + */ MatrixXd pose_jacobian(const VectorXd& q, const int& to_link) const override; + /** + * @brief Computes the full constrained pose Jacobian. + * + * @param q Configuration vector [x, y, phi]^T. + * @return The full constrained pose Jacobian. + */ MatrixXd pose_jacobian(const VectorXd &q) const override; + /** + * @brief Computes the time derivative of the constrained pose Jacobian. + * + * @param q Configuration vector [x, y, phi]^T. + * @param q_dot Configuration-velocity vector [x_dot, y_dot, phi_dot]^T. + * @param to_link Column index of the partial Jacobian derivative to be returned. + * @return The constrained pose-Jacobian derivative up to the requested column. + * @throws std::runtime_error If @p to_link is not 0 or 1. + */ MatrixXd pose_jacobian_derivative(const VectorXd& q, const VectorXd& q_dot, const int& to_link) const override; + /** + * @brief Computes the full time derivative of the constrained pose Jacobian. + * + * @param q Configuration vector [x, y, phi]^T. + * @param q_dot Configuration-velocity vector [x_dot, y_dot, phi_dot]^T. + * @return The full constrained pose-Jacobian derivative. + */ MatrixXd pose_jacobian_derivative (const VectorXd& q, const VectorXd& q_dot) const override; }; diff --git a/include/dqrobotics/robot_modeling/DQ_HolonomicBase.h b/include/dqrobotics/robot_modeling/DQ_HolonomicBase.h index eff0407..ea4eda4 100644 --- a/include/dqrobotics/robot_modeling/DQ_HolonomicBase.h +++ b/include/dqrobotics/robot_modeling/DQ_HolonomicBase.h @@ -30,23 +30,108 @@ namespace DQ_robotics { +/** + * @brief Basic implementation of a holonomic mobile base. + * + * The configuration vector is q = [x, y, phi]^T, where x and y describe the + * planar position and phi is the planar orientation. The concrete methods in + * this class compute the raw pose and raw Jacobians of that planar motion and + * optionally account for an additional frame displacement. + * + * @see DQ_MobileBase, DQ_DifferentialDriveRobot + */ class DQ_HolonomicBase: public DQ_MobileBase { public: + /** + * @brief Constructs a holonomic base. + * + * The configuration space has dimension three. + */ DQ_HolonomicBase(); //Virtual method overloads (DQ_Kinematics) + /** + * @brief Computes the mobile-base pose while considering the frame displacement. + * + * @param q Configuration vector [x, y, phi]^T. + * @return The mobile-base pose as a unit dual quaternion. + */ virtual DQ fkm(const VectorXd& q) const override; + /** + * @brief Computes the mobile-base pose while considering the frame displacement. + * + * This overload exists for compatibility with the generic kinematic interface. + * The current implementation accepts only @p to_ith_link = 2. + * + * @param q Configuration vector [x, y, phi]^T. + * @param to_ith_link Link index. + * @return The mobile-base pose as a unit dual quaternion. + * @throws std::runtime_error If @p to_ith_link is different from 2. + */ virtual DQ fkm(const VectorXd& q, const int& to_ith_link) const override; + /** + * @brief Computes the pose Jacobian while considering the frame displacement. + * + * @param q Configuration vector [x, y, phi]^T. + * @param to_link Column index of the partial Jacobian to be returned. + * @return The pose Jacobian up to the requested column. + * @throws std::runtime_error If @p to_link is outside the interval [0, 2]. + */ virtual MatrixXd pose_jacobian(const VectorXd& q, const int& to_link) const override; + /** + * @brief Computes the full pose Jacobian while considering the frame displacement. + * + * @param q Configuration vector [x, y, phi]^T. + * @return The full pose Jacobian. + */ virtual MatrixXd pose_jacobian(const VectorXd& q) const override; + /** + * @brief Computes the time derivative of the pose Jacobian while considering the frame displacement. + * + * @param q Configuration vector [x, y, phi]^T. + * @param q_dot Configuration-velocity vector [x_dot, y_dot, phi_dot]^T. + * @param to_link Column index of the partial Jacobian derivative to be returned. + * @return The pose-Jacobian derivative up to the requested column. + * @throws std::runtime_error If @p to_link is outside the interval [0, 2]. + */ virtual MatrixXd pose_jacobian_derivative(const VectorXd& q, const VectorXd& q_dot, const int& to_link) const override; + /** + * @brief Computes the full time derivative of the pose Jacobian. + * + * @param q Configuration vector [x, y, phi]^T. + * @param q_dot Configuration-velocity vector [x_dot, y_dot, phi_dot]^T. + * @return The full pose-Jacobian derivative. + */ virtual MatrixXd pose_jacobian_derivative(const VectorXd& q, const VectorXd& q_dot) const override; + /** + * @brief Computes the planar mobile-base pose without considering the frame displacement. + * + * @param q Configuration vector [x, y, phi]^T. + * @return The raw mobile-base pose as a unit dual quaternion. + */ DQ raw_fkm(const VectorXd& q) const; + /** + * @brief Computes the raw pose Jacobian of the planar mobile base. + * + * @param q Configuration vector [x, y, phi]^T. + * @param to_link Column index of the partial Jacobian to be returned. + * @return The raw pose Jacobian up to the requested column. + * @throws std::runtime_error If @p to_link is outside the interval [0, 2]. + */ MatrixXd raw_pose_jacobian(const VectorXd& q, const int& to_link=2) const; + /** + * @brief Computes the raw time derivative of the pose Jacobian of the planar mobile base. + * + * @param q Configuration vector [x, y, phi]^T. + * @param q_dot Configuration-velocity vector [x_dot, y_dot, phi_dot]^T. + * @param to_link Column index of the partial Jacobian derivative to be returned. + * @return The raw pose-Jacobian derivative up to the requested column. + * @throws std::runtime_error If @p to_link is outside the interval [0, 2]. + */ MatrixXd raw_pose_jacobian_derivative(const VectorXd& q, const VectorXd& q_dot, const int& to_link = 2) const; }; diff --git a/include/dqrobotics/robot_modeling/DQ_JointType.h b/include/dqrobotics/robot_modeling/DQ_JointType.h index 102ad38..62efdad 100644 --- a/include/dqrobotics/robot_modeling/DQ_JointType.h +++ b/include/dqrobotics/robot_modeling/DQ_JointType.h @@ -27,45 +27,60 @@ This file is part of DQ Robotics. namespace DQ_robotics { +/** + * @brief Encodes the actuation type of a robot joint. + * + * The available joint types follow Table 1 of Silva, Quiroz-Omaña, and + * Adorno (2022), “Dynamics of Mobile Manipulators Using Dual Quaternion Algebra”. + * The wrapper allows type-safe comparisons while still supporting conversion + * from integral values used in matrix-based robot descriptions. + * + * @see DQ_SerialManipulator + */ class DQ_JointType { public: + /** @brief Enumeration of supported joint types. */ enum JOINT_TYPE{ - REVOLUTE = 0, - PRISMATIC, - SPHERICAL, - CYLINDRICAL, - PLANAR, - SIX_DOF, - HELICAL + REVOLUTE = 0, /**< Revolute joint. */ + PRISMATIC, /**< Prismatic joint. */ + SPHERICAL, /**< Spherical joint. */ + CYLINDRICAL, /**< Cylindrical joint. */ + PLANAR, /**< Planar joint. */ + SIX_DOF, /**< Six-degree-of-freedom joint. */ + HELICAL /**< Helical joint. */ }; - // This definition enables switch cases and comparisons. + /** + * @brief Converts the object to its underlying enumeration value. + * + * @return The stored joint-type enumeration. + */ constexpr operator JOINT_TYPE() const { return joint_type_; } private: + /** @brief Stored joint-type value. */ JOINT_TYPE joint_type_; public: /** - * @brief DQ_JointType Default constructor method. - * This class is based on Table 1 of Silva, Quiroz-Omaña, and Adorno (2022). - * Dynamics of Mobile Manipulators Using Dual Quaternion Algebra. + * @brief Default constructor. */ DQ_JointType() = default; /** - * @brief DQ_JointType Constructor method. - * This class is based on Table 1 of Silva, Quiroz-Omaña, and Adorno (2022). - * Dynamics of Mobile Manipulators Using Dual Quaternion Algebra. - * @param joint_type The joint type. Example: REVOLUTE, PRISMATIC, - * SPHERICAL, CYLINDRICAL, PLANAR, SIX_DOF, or HELICAL. + * @brief Constructs the joint type from an enumeration value. + * + * @param joint_type Desired joint type. */ DQ_JointType(const JOINT_TYPE& joint_type): joint_type_{joint_type}{}; /** - * @brief DQ_JointType Constructor method that allows integer arguments. - * This class is based on Table 1 of Silva, Quiroz-Omaña, and Adorno (2022). - * Dynamics of Mobile Manipulators Using Dual Quaternion Algebra - * @param joint_type The joint type. + * @brief Constructs the joint type from an integer code. + * + * The accepted values are 0 for REVOLUTE, 1 for PRISMATIC, 2 for SPHERICAL, + * 3 for CYLINDRICAL, 4 for PLANAR, 5 for SIX_DOF, and 6 for HELICAL. + * + * @param joint_type Integer code of the desired joint type. + * @throws std::runtime_error If @p joint_type is outside the supported range. */ DQ_JointType(const int& joint_type){ switch (joint_type) { @@ -96,8 +111,10 @@ class DQ_JointType } /** - * @brief to_string() converts the DQ_JointType to string. - * @return A string that corresponds to the joint type. + * @brief Converts the joint type to its uppercase string representation. + * + * @return String corresponding to the stored joint type. + * @throws std::runtime_error If the stored value does not match a supported joint type. */ std::string to_string() const { switch (joint_type_) { diff --git a/include/dqrobotics/robot_modeling/DQ_Kinematics.h b/include/dqrobotics/robot_modeling/DQ_Kinematics.h index fbd52b3..327f744 100644 --- a/include/dqrobotics/robot_modeling/DQ_Kinematics.h +++ b/include/dqrobotics/robot_modeling/DQ_Kinematics.h @@ -27,92 +27,444 @@ This file is part of DQ Robotics. namespace DQ_robotics { +/** + * @brief Abstract class that defines an interface to implement robot kinematics. + * + * DQ_Kinematics is the common base class for robot models in DQ Robotics. + * It stores the reference and base frames, the dimension of the configuration + * space, and static operations that derive task-space Jacobians from a pose + * Jacobian represented in dual quaternion form. + * + * @note The methods declared with = 0 are pure virtual interface contracts and + * must be implemented by subclasses. The virtual overloads without an explicit + * link index are concrete convenience wrappers that default to the last link. + * + * @see DQ_SerialManipulator, DQ_MobileBase, DQ_CooperativeDualTaskSpace + */ class DQ_Kinematics { protected: + /** @brief Name assigned to the kinematic model. */ std::string name_; + /** @brief Reference frame used by fkm() and pose_jacobian() computations. */ DQ reference_frame_; + /** @brief Physical placement of the robot base in the workspace. */ DQ base_frame_; + /** @brief Configuration vector associated with the model state. */ VectorXd q_; + /** @brief Dimension of the configuration space. */ int dim_configuration_space_; + /** @brief Lower joint-position limits. */ VectorXd lower_q_limit_; + /** @brief Upper joint-position limits. */ VectorXd upper_q_limit_; + /** @brief Lower joint-velocity limits. */ VectorXd lower_q_dot_limit_; + /** @brief Upper joint-velocity limits. */ VectorXd upper_q_dot_limit_; + /** + * @brief Checks whether a link index is valid for this model. + * + * @param to_ith_link Index of the link to be checked. + * @throws std::runtime_error If @p to_ith_link is outside the valid range. + */ void _check_to_ith_link(const int& to_ith_link) const; + /** + * @brief Checks whether a configuration vector has the correct dimension. + * + * @param q_vec Vector to be validated. + * @throws std::runtime_error If @p q_vec does not match the configuration-space dimension. + */ void _check_q_vec(const VectorXd& q_vec) const; //Constructor + /** @brief Constructs a kinematic model with identity reference and base frames. */ DQ_Kinematics(); public: //Virtual destructor + /** @brief Virtual destructor. */ virtual ~DQ_Kinematics() = default; //Concrete methods + /** + * @brief Sets the reference frame used by the forward kinematics and Jacobian methods. + * + * @param get_reference_frame Unit dual quaternion representing the reference frame. + * @throws std::runtime_error If @p get_reference_frame is not a unit dual quaternion. + */ void set_reference_frame(const DQ& get_reference_frame); + /** + * @brief Returns the current reference frame. + * + * @return The reference frame as a unit dual quaternion. + */ DQ get_reference_frame() const; + /** + * @brief Sets the physical base frame of the robot in the workspace. + * + * The base frame determines the physical placement of the robot and does not + * need to coincide with the reference frame used for calculations. + * + * @param get_base_frame Unit dual quaternion representing the base frame. + * @throws std::runtime_error If @p get_base_frame is not a unit dual quaternion. + */ void set_base_frame(const DQ& get_base_frame); + /** + * @brief Returns the current base frame. + * + * @return The base frame as a unit dual quaternion. + */ DQ get_base_frame() const; + /** + * @brief Sets the name of the kinematic model. + * + * @param get_name Name to be assigned to the model. + */ void set_name(const std::string& get_name); + /** + * @brief Returns the model name. + * + * @return The current model name. + */ std::string get_name() const; //PURE virtual methods + /** + * @brief Computes the forward kinematics up to the last link. + * + * This is a pure virtual interface contract and must be implemented by subclasses. + * + * @param joint_configurations Vector containing the robot joint configurations. + * @return The pose of the last link, represented as a unit dual quaternion. + */ virtual DQ fkm (const VectorXd& joint_configurations) const = 0; + /** + * @brief Computes the forward kinematics up to a given link. + * + * This is a pure virtual interface contract and must be implemented by subclasses. + * + * @param joint_configurations Vector containing the robot joint configurations. + * @param to_ith_link Index of the last link to be accounted for in the computation. + * @return The pose of the reference frame attached to the ith link, represented as a unit dual quaternion. + */ virtual DQ fkm (const VectorXd& joint_configurations, const int& to_ith_link) const = 0; + /** + * @brief Computes the pose Jacobian up to a given link. + * + * This is a pure virtual interface contract and must be implemented by subclasses. + * + * @param joint_configurations Vector containing the robot joint configurations. + * @param to_ith_link Index of the last link to be accounted for in the computation. + * @return The pose Jacobian that satisfies vec8(pose_dot) = J * q_dot. + */ virtual MatrixXd pose_jacobian(const VectorXd& joint_configurations, const int& to_ith_link) const = 0; + /** + * @brief Computes the time derivative of the pose Jacobian up to a given link. + * + * This is a pure virtual interface contract and must be implemented by subclasses. + * + * @param q Vector containing the robot joint configurations. + * @param q_dot Vector containing the robot joint velocities. + * @param to_ith_link Index of the last link to be accounted for in the computation. + * @return The Jacobian derivative that satisfies vec8(pose_ddot) = J_dot * q_dot + J * q_ddot. + */ virtual MatrixXd pose_jacobian_derivative(const VectorXd& q, const VectorXd& q_dot, const int& to_ith_link) const = 0; //Virtual methods + /** + * @brief Computes the pose Jacobian up to the last link. + * + * This concrete overload delegates to pose_jacobian(joint_configurations, get_dim_configuration_space() - 1). + * + * @param joint_configurations Vector containing the robot joint configurations. + * @return The pose Jacobian that satisfies vec8(pose_dot) = J * q_dot. + */ virtual MatrixXd pose_jacobian (const VectorXd& joint_configurations) const; + /** + * @brief Computes the time derivative of the pose Jacobian up to the last link. + * + * This concrete overload delegates to pose_jacobian_derivative(q, q_dot, get_dim_configuration_space() - 1). + * + * @param q Vector containing the robot joint configurations. + * @param q_dot Vector containing the robot joint velocities. + * @return The Jacobian derivative that satisfies vec8(pose_ddot) = J_dot * q_dot + J * q_ddot. + */ virtual MatrixXd pose_jacobian_derivative(const VectorXd& q, const VectorXd& q_dot) const; + /** + * @brief Returns the dimension of the configuration space. + * + * @return Number of generalized coordinates of the model. + */ virtual int get_dim_configuration_space() const; //Static methods + /** + * @brief Computes the squared-distance Jacobian from a pose Jacobian. + * + * @param pose_jacobian Pose Jacobian associated with @p pose. + * @param pose Pose associated with @p pose_jacobian. + * @return The Jacobian of the squared distance from the pose origin to the reference-frame origin. + */ static MatrixXd distance_jacobian (const MatrixXd& pose_jacobian, const DQ& pose); + /** + * @brief Computes the translation Jacobian from a pose Jacobian. + * + * @param pose_jacobian Pose Jacobian associated with @p pose. + * @param pose Pose associated with @p pose_jacobian. + * @return The Jacobian that satisfies vec4(translation_dot) = J * q_dot. + */ static MatrixXd translation_jacobian (const MatrixXd& pose_jacobian, const DQ& pose); + /** + * @brief Extracts the rotation Jacobian from a pose Jacobian. + * + * @param pose_jacobian Pose Jacobian in dual quaternion form. + * @return The Jacobian that satisfies vec4(rotation_dot) = J * q_dot. + */ static MatrixXd rotation_jacobian (const MatrixXd& pose_jacobian); + /** + * @brief Computes the Jacobian of a line rigidly attached to a pose. + * + * @param pose_jacobian Pose Jacobian associated with @p pose. + * @param pose Pose associated with @p pose_jacobian. + * @param line_direction Line direction expressed in the local frame of @p pose. + * @return The line Jacobian of the line obtained by transforming @p line_direction with @p pose. + */ static MatrixXd line_jacobian (const MatrixXd& pose_jacobian, const DQ& pose, const DQ& line_direction); + /** + * @brief Computes the Jacobian of a plane rigidly attached to a pose. + * + * @param pose_jacobian Pose Jacobian associated with @p pose. + * @param pose Pose associated with @p pose_jacobian. + * @param plane_normal Plane normal expressed in the local frame of @p pose. + * @return The plane Jacobian of the plane obtained by transforming @p plane_normal with @p pose. + */ static MatrixXd plane_jacobian (const MatrixXd& pose_jacobian, const DQ& pose, const DQ& plane_normal); + /** + * @brief Extracts the rotation-Jacobian derivative from a pose-Jacobian derivative. + * + * @param pose_jacobian_derivative Pose-Jacobian derivative in dual quaternion form. + * @return The derivative of the rotation Jacobian. + */ static MatrixXd rotation_jacobian_derivative (const MatrixXd& pose_jacobian_derivative); + /** + * @brief Computes the time derivative of the translation Jacobian. + * + * @param pose_jacobian Pose Jacobian associated with @p pose. + * @param pose_jacobian_derivative Time derivative of @p pose_jacobian. + * @param pose Pose associated with @p pose_jacobian. + * @param q_dot Vector containing the robot configuration velocities. + * @return The derivative of the translation Jacobian. + */ static MatrixXd translation_jacobian_derivative (const MatrixXd& pose_jacobian, const MatrixXd& pose_jacobian_derivative, const DQ& pose, const VectorXd &q_dot); + /** + * @brief Computes the time derivative of the squared-distance Jacobian. + * + * @param pose_jacobian Pose Jacobian associated with @p pose. + * @param pose_jacobian_derivative Time derivative of @p pose_jacobian. + * @param pose Pose associated with @p pose_jacobian. + * @param q_dot Vector containing the robot configuration velocities. + * @return The derivative of the squared-distance Jacobian. + */ static MatrixXd distance_jacobian_derivative (const MatrixXd& pose_jacobian, const MatrixXd& pose_jacobian_derivative, const DQ& pose, const VectorXd &q_dot); + /** + * @brief Computes the time derivative of a plane Jacobian. + * + * @param pose_jacobian Pose Jacobian associated with @p pose. + * @param pose_jacobian_derivative Time derivative of @p pose_jacobian. + * @param pose Pose associated with @p pose_jacobian. + * @param plane_normal Plane normal expressed in the local frame of @p pose. + * @param q_dot Vector containing the robot configuration velocities. + * @return The derivative of the plane Jacobian. + */ static MatrixXd plane_jacobian_derivative (const MatrixXd& pose_jacobian, const MatrixXd& pose_jacobian_derivative, const DQ& pose, const DQ& plane_normal, const VectorXd &q_dot); + /** + * @brief Computes the time derivative of a line Jacobian. + * + * @param pose_jacobian Pose Jacobian associated with @p pose. + * @param pose_jacobian_derivative Time derivative of @p pose_jacobian. + * @param pose Pose associated with @p pose_jacobian. + * @param line_direction Line direction expressed in the local frame of @p pose. + * @param q_dot Vector containing the robot configuration velocities. + * @return The derivative of the line Jacobian. + */ static MatrixXd line_jacobian_derivative (const MatrixXd& pose_jacobian, const MatrixXd& pose_jacobian_derivative, const DQ& pose, const DQ& line_direction, const VectorXd &q_dot); + /** + * @brief Computes the squared point-to-point distance Jacobian. + * + * @param translation_jacobian Translation Jacobian of the robot point. + * @param robot_point Point rigidly attached to the robot, represented as a pure quaternion. + * @param workspace_point Workspace point, represented as a pure quaternion. + * @return The squared point-to-point distance Jacobian. + * @throws std::range_error If either point is not a pure quaternion. + */ static MatrixXd point_to_point_distance_jacobian(const MatrixXd& translation_jacobian, const DQ& robot_point, const DQ& workspace_point); + /** + * @brief Computes the residual term of the squared point-to-point distance dynamics. + * + * @param robot_point Point rigidly attached to the robot, represented as a pure quaternion. + * @param workspace_point Workspace point, represented as a pure quaternion. + * @param workspace_point_derivative Time derivative of the workspace point. + * @return The residual term associated with the workspace-point motion. + * @throws std::range_error If @p robot_point or @p workspace_point is not a pure quaternion. + */ static double point_to_point_residual (const DQ& robot_point, const DQ& workspace_point, const DQ& workspace_point_derivative); + /** + * @brief Computes the squared point-to-line distance Jacobian. + * + * @param translation_jacobian Translation Jacobian of the robot point. + * @param robot_point Point rigidly attached to the robot, represented as a pure quaternion. + * @param workspace_line Workspace line. + * @return The squared point-to-line distance Jacobian. + * @throws std::range_error If @p robot_point is not a pure quaternion or @p workspace_line is not a line. + */ static MatrixXd point_to_line_distance_jacobian (const MatrixXd& translation_jacobian, const DQ& robot_point, const DQ& workspace_line); + /** + * @brief Computes the residual term of the squared point-to-line distance dynamics. + * + * @param robot_point Point rigidly attached to the robot, represented as a pure quaternion. + * @param workspace_line Workspace line. + * @param workspace_line_derivative Time derivative of the workspace line. + * @return The residual term associated with the workspace-line motion. + * @throws std::range_error If @p robot_point is not a pure quaternion or @p workspace_line is not a line. + */ static double point_to_line_residual (const DQ& robot_point, const DQ& workspace_line, const DQ& workspace_line_derivative); + /** + * @brief Computes the squared point-to-plane distance Jacobian. + * + * @param translation_jacobian Translation Jacobian of the robot point. + * @param robot_point Point rigidly attached to the robot, represented as a pure quaternion. + * @param workspace_plane Workspace plane. + * @return The squared point-to-plane distance Jacobian. + * @throws std::range_error If @p robot_point is not a pure quaternion or @p workspace_plane is not a plane. + */ static MatrixXd point_to_plane_distance_jacobian(const MatrixXd& translation_jacobian, const DQ& robot_point, const DQ& workspace_plane); + /** + * @brief Computes the residual term of the squared point-to-plane distance dynamics. + * + * @param translation Translation of the robot point, represented as a pure quaternion. + * @param plane_derivative Time derivative of the workspace plane. + * @return The residual term associated with the workspace-plane motion. + * @throws std::range_error If @p translation is not a pure quaternion. + */ static double point_to_plane_residual (const DQ& translation, const DQ& plane_derivative); + /** + * @brief Computes the squared line-to-point distance Jacobian. + * + * @param line_jacobian Line Jacobian of the robot line. + * @param robot_line Line rigidly attached to the robot. + * @param workspace_point Workspace point, represented as a pure quaternion. + * @return The squared line-to-point distance Jacobian. + * @throws std::range_error If @p robot_line is not a line or @p workspace_point is not a pure quaternion. + */ static MatrixXd line_to_point_distance_jacobian (const MatrixXd& line_jacobian, const DQ& robot_line, const DQ& workspace_point); + /** + * @brief Computes the residual term of the squared line-to-point distance dynamics. + * + * @param robot_line Line rigidly attached to the robot. + * @param workspace_point Workspace point, represented as a pure quaternion. + * @param workspace_point_derivative Time derivative of the workspace point. + * @return The residual term associated with the workspace-point motion. + * @throws std::range_error If @p robot_line is not a line or @p workspace_point is not a pure quaternion. + */ static double line_to_point_residual (const DQ& robot_line, const DQ& workspace_point, const DQ& workspace_point_derivative); + /** + * @brief Computes the squared line-to-line distance Jacobian. + * + * @param line_jacobian Line Jacobian of the robot line. + * @param robot_line Line rigidly attached to the robot. + * @param workspace_line Workspace line. + * @return The squared line-to-line distance Jacobian. + * @throws std::range_error If @p robot_line or @p workspace_line is not a line. + */ static MatrixXd line_to_line_distance_jacobian (const MatrixXd& line_jacobian, const DQ& robot_line, const DQ& workspace_line); + /** + * @brief Computes the residual term of the squared line-to-line distance dynamics. + * + * @param robot_line Line rigidly attached to the robot. + * @param workspace_line Workspace line. + * @param workspace_line_derivative Time derivative of the workspace line. + * @return The residual term associated with the workspace-line motion. + * @throws std::range_error If @p robot_line or @p workspace_line is not a line. + */ static double line_to_line_residual (const DQ& robot_line, const DQ& workspace_line, const DQ& workspace_line_derivative); + /** + * @brief Computes the squared plane-to-point distance Jacobian. + * + * @param plane_jacobian Plane Jacobian of the robot plane. + * @param workspace_point Workspace point, represented as a pure quaternion. + * @return The squared plane-to-point distance Jacobian. + * @throws std::range_error If @p workspace_point is not a pure quaternion. + */ static MatrixXd plane_to_point_distance_jacobian(const MatrixXd& plane_jacobian, const DQ& workspace_point); + /** + * @brief Computes the residual term of the squared plane-to-point distance dynamics. + * + * @param robot_plane Plane rigidly attached to the robot. + * @param workspace_point_derivative Time derivative of the workspace point. + * @return The residual term associated with the workspace-point motion. + * @throws std::range_error If @p workspace_point_derivative is not a pure quaternion. + */ static double plane_to_point_residual (const DQ& robot_plane, const DQ& workspace_point_derivative); + /** + * @brief Computes the Jacobian of the line-to-line angle objective. + * + * @param line_jacobian Line Jacobian of the robot line. + * @param robot_line Line rigidly attached to the robot. + * @param workspace_line Workspace line. + * @return The Jacobian associated with the objective f(phi) = dot(robot_line - workspace_line, robot_line - workspace_line). + * @throws std::range_error If @p robot_line or @p workspace_line is not a line. + */ static MatrixXd line_to_line_angle_jacobian (const MatrixXd& line_jacobian, const DQ& robot_line, const DQ& workspace_line); + /** + * @brief Computes the residual term of the line-to-line angle objective. + * + * @param robot_line Line rigidly attached to the robot. + * @param workspace_line Workspace line. + * @param workspace_line_derivative Time derivative of the workspace line. + * @return The residual term associated with workspace-line motion. + * @throws std::range_error If @p robot_line or @p workspace_line is not a line. + */ static double line_to_line_angle_residual (const DQ& robot_line, const DQ& workspace_line, const DQ& workspace_line_derivative); + /** + * @brief Computes a squared-distance Jacobian between two line segments. + * + * The method selects the appropriate point-to-point, point-to-line, or + * line-to-line Jacobian according to the closest elements of the two + * segments, following the active-constraints formulation used in DQ Robotics. + * + * @param line_jacobian Line Jacobian of @p robot_line. + * @param robot_point_1_translation_jacobian Translation Jacobian of the first endpoint of the robot segment. + * @param robot_point_2_translation_jacobian Translation Jacobian of the second endpoint of the robot segment. + * @param robot_line Line containing the robot segment. + * @param robot_point_1 First endpoint of the robot segment. + * @param robot_point_2 Second endpoint of the robot segment. + * @param workspace_line Line containing the workspace segment. + * @param workspace_point_1 First endpoint of the workspace segment. + * @param workspace_point_2 Second endpoint of the workspace segment. + * @return The squared-distance Jacobian between the two segments. + * @throws std::runtime_error If the provided line-segment data is inconsistent or an unexpected closest-element case is reached. + */ static MatrixXd line_segment_to_line_segment_distance_jacobian(const MatrixXd& line_jacobian, const MatrixXd& robot_point_1_translation_jacobian, const MatrixXd& robot_point_2_translation_jacobian, diff --git a/include/dqrobotics/robot_modeling/DQ_MobileBase.h b/include/dqrobotics/robot_modeling/DQ_MobileBase.h index 04924d7..41680c4 100644 --- a/include/dqrobotics/robot_modeling/DQ_MobileBase.h +++ b/include/dqrobotics/robot_modeling/DQ_MobileBase.h @@ -29,13 +29,28 @@ This file is part of DQ Robotics. namespace DQ_robotics { +/** + * @brief Abstract class that defines an interface for mobile bases. + * + * DQ_MobileBase specializes DQ_Kinematics for mobile robots whose pose is + * typically described by a low-dimensional configuration vector and an + * additional rigid displacement from the planar base pose to the actual base frame. + * + * @note This class remains abstract because the kinematic interface inherited + * from DQ_Kinematics must be implemented by subclasses. + * + * @see DQ_Kinematics, DQ_HolonomicBase + */ class DQ_MobileBase : public DQ_Kinematics { protected: + /** @brief Constant rigid displacement from the raw mobile-base pose to the base frame. */ DQ frame_displacement_; + /** @brief Constructs a mobile base with identity frame displacement. */ DQ_MobileBase(); public: + /** @brief Virtual destructor. */ virtual ~DQ_MobileBase() = default; //Abstract methods (Inherited from DQ_Kinematics) @@ -44,7 +59,17 @@ class DQ_MobileBase : public DQ_Kinematics //virtual MatrixXd pose_jacobian(const VectorXd& joint_configurations,const int& to_link) const = 0; //virtual MatrixXd pose_jacobian_derivative(const VectorXd& q, const VectorXd& q_dot, const int& to_link) const = 0; + /** + * @brief Sets the rigid displacement from the raw mobile-base pose to the base frame. + * + * @param pose Constant rigid displacement represented as a dual quaternion. + */ void set_frame_displacement(const DQ& pose); + /** + * @brief Returns the rigid displacement from the raw mobile-base pose to the base frame. + * + * @return The stored frame displacement. + */ DQ frame_displacement(); }; diff --git a/include/dqrobotics/robot_modeling/DQ_ParameterDH.h b/include/dqrobotics/robot_modeling/DQ_ParameterDH.h index 696a650..bf7c60b 100644 --- a/include/dqrobotics/robot_modeling/DQ_ParameterDH.h +++ b/include/dqrobotics/robot_modeling/DQ_ParameterDH.h @@ -27,17 +27,29 @@ This file is part of DQ Robotics. namespace DQ_robotics { +/** + * @brief Selects a Denavit-Hartenberg parameter by name. + * + * DQ_ParameterDH is a small wrapper used by the C++ API to refer to the + * rows of a DH or modified-DH matrix. It also accepts string-based + * construction to keep the interface aligned with other DQ Robotics bindings. + * + * @see DQ_SerialManipulatorDH, DQ_SerialManipulatorMDH + */ class DQ_ParameterDH { public: + /** @brief Enumeration of supported Denavit-Hartenberg parameters. */ enum PARAMETER{ - THETA, - D, - A, - ALPHA + THETA, /**< Joint-angle parameter. */ + D, /**< Link offset parameter. */ + A, /**< Link-length parameter. */ + ALPHA /**< Link-twist parameter. */ }; private: + /** @brief Stored Denavit-Hartenberg parameter. */ PARAMETER parameter_; + /** @brief Mapping from uppercase strings to supported Denavit-Hartenberg parameters. */ const std::unordered_map map_ = {{"THETA", THETA}, {"D" , D}, @@ -46,8 +58,10 @@ class DQ_ParameterDH }; /** - * @brief _get_parameter sets the parameter member using a string as argument. - * @param parameter The desired parameter to be set. Example: "THETA", "D", "A", or "ALPHA". + * @brief Sets the stored parameter from a string. + * + * @param parameter Name of the desired parameter. + * @throws std::runtime_error If @p parameter is not one of THETA, D, A, or ALPHA. */ void _set_parameter(const std::string& parameter) { @@ -59,25 +73,32 @@ class DQ_ParameterDH } public: /** - * @brief DQ_ParameterDH Default constructor method. + * @brief Default constructor. */ DQ_ParameterDH() = default; /** - * @brief DQ_ParameterDH Constructor method - * @param parameter The desired DH parameter. Example: THETA, D, A, or ALPHA. + * @brief Constructs the selector from an enumeration value. + * + * @param parameter Desired DH parameter. */ DQ_ParameterDH(const PARAMETER& parameter): parameter_{parameter}{}; - // This definition enables switch cases and comparisons. + /** + * @brief Converts the object to its underlying enumeration value. + * + * @return The stored parameter enumeration. + */ constexpr operator PARAMETER() const { return parameter_; } /** - * @brief DQ_ParameterDH Constructor method that allows string parameters. - * This is done to keep the language compatibility between - * Matlab and Python/C++, as discussed in - * https://github.com/dqrobotics/cpp/pull/69 - * @param parameter The desired DH parameter. Example: "THETA", "D", "A", or "ALPHA". + * @brief Constructs the selector from a string. + * + * This constructor keeps the C++ interface compatible with string-based + * parameter selection used in other DQ Robotics language bindings. + * + * @param parameter Desired DH parameter as a string. + * @throws std::runtime_error If @p parameter is not one of THETA, D, A, or ALPHA. */ DQ_ParameterDH(const std::string& parameter){ _set_parameter(parameter); @@ -85,11 +106,13 @@ class DQ_ParameterDH /** - * @brief DQ_ParameterDH Constructor method that allows char parameters. - * This is done to keep the language compatibility between - * Matlab and Python/C++, as discussed in - * https://github.com/dqrobotics/cpp/pull/69 - * @param parameter_c The desired DH parameter. Example: "THETA", "D", "A", or "ALPHA". + * @brief Constructs the selector from a C string. + * + * This constructor keeps the C++ interface compatible with string-based + * parameter selection used in other DQ Robotics language bindings. + * + * @param parameter_c Desired DH parameter as a C string. + * @throws std::runtime_error If @p parameter_c is not one of THETA, D, A, or ALPHA. */ DQ_ParameterDH(const char* parameter_c){ _set_parameter(parameter_c); diff --git a/include/dqrobotics/robot_modeling/DQ_SerialManipulator.h b/include/dqrobotics/robot_modeling/DQ_SerialManipulator.h index 651a815..d27e35f 100644 --- a/include/dqrobotics/robot_modeling/DQ_SerialManipulator.h +++ b/include/dqrobotics/robot_modeling/DQ_SerialManipulator.h @@ -1,86 +1,299 @@ -#pragma once -/** -(C) Copyright 2011-2025 DQ Robotics Developers - -This file is part of DQ Robotics. - - DQ Robotics is free software: you can redistribute it and/or modify - it under the terms of the GNU Lesser General Public License as published by - the Free Software Foundation, either version 3 of the License, or - (at your option) any later version. - - DQ Robotics is distributed in the hope that it will be useful, - but WITHOUT ANY WARRANTY; without even the implied warranty of - MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the - GNU Lesser General Public License for more details. - - You should have received a copy of the GNU Lesser General Public License - along with DQ Robotics. If not, see . - -Contributors: -1. Murilo M. Marinho (murilomarinho@ieee.org) -2. Mateus Rodrigues Martins (martinsrmateus@gmail.com) - -3. Juan Jose Quiroz Omana (juanjose.quirozomana@manchester.ac.uk) - - Added the joint_types member, and the following methods: - _check_joint_types(), and {set,get}_joint_{type, types}. -*/ - -#include -#include -#include - -namespace DQ_robotics -{ - -class DQ_SerialManipulator: public DQ_Kinematics -{ -protected: - DQ curr_effector_; - std::vector joint_types_; - DQ_SerialManipulator(const int& dofs); - void _check_joint_types() const; -public: - DQ get_effector() const; - DQ set_effector(const DQ& new_effector); - - VectorXd get_lower_q_limit() const; - void set_lower_q_limit(const VectorXd& lower_q_limit); - VectorXd get_lower_q_dot_limit() const; - void set_lower_q_dot_limit(const VectorXd &lower_q_dot_limit); - VectorXd get_upper_q_limit() const; - void set_upper_q_limit(const VectorXd& upper_q_limit); - VectorXd get_upper_q_dot_limit() const; - void set_upper_q_dot_limit(const VectorXd &upper_q_dot_limit); - - DQ_JointType get_joint_type(const int& ith_joint) const; - std::vector get_joint_types() const; - void set_joint_type(const DQ_JointType& joint_type, const int& ith_joint); - void set_joint_types(const std::vector& joint_types); - void set_joint_types(const VectorXd& joint_types); - - //Virtual - virtual MatrixXd raw_pose_jacobian(const VectorXd& q_vec) const; - virtual MatrixXd raw_pose_jacobian_derivative(const VectorXd& q, const VectorXd& q_dot) const; - virtual DQ raw_fkm(const VectorXd& q_vec) const; - - //Pure virtual - virtual MatrixXd raw_pose_jacobian(const VectorXd& q_vec, const int& to_ith_link) const = 0; - virtual MatrixXd raw_pose_jacobian_derivative(const VectorXd& q, const VectorXd& q_dot, const int& to_ith_link) const = 0; - virtual DQ raw_fkm(const VectorXd& q_vec, const int& to_ith_link) const = 0; - virtual std::vector get_supported_joint_types() const = 0; - - //Overrides from DQ_Kinematics - virtual DQ fkm(const VectorXd& q_vec) const override; //Override from DQ_Kinematics - virtual DQ fkm(const VectorXd& q_vec, const int& to_ith_link) const override; //Override from DQ_Kinematics - - virtual int get_dim_configuration_space() const override; //Override from DQ_Kinematics - - virtual MatrixXd pose_jacobian(const VectorXd& q_vec, const int& to_ith_link) const override; //Override from DQ_Kinematics - virtual MatrixXd pose_jacobian(const VectorXd& q_vec) const override; //Override from DQ_Kinematics - virtual MatrixXd pose_jacobian_derivative(const VectorXd& q, const VectorXd& q_dot, const int& to_ith_link) const override; //Override from DQ_Kinematics - virtual MatrixXd pose_jacobian_derivative(const VectorXd& q, const VectorXd& q_dot) const override; //Override from DQ_Kinematics - -}; - -} +#pragma once +/** +(C) Copyright 2011-2025 DQ Robotics Developers + +This file is part of DQ Robotics. + + DQ Robotics is free software: you can redistribute it and/or modify + it under the terms of the GNU Lesser General Public License as published by + the Free Software Foundation, either version 3 of the License, or + (at your option) any later version. + + DQ Robotics is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU Lesser General Public License for more details. + + You should have received a copy of the GNU Lesser General Public License + along with DQ Robotics. If not, see . + +Contributors: +1. Murilo M. Marinho (murilomarinho@ieee.org) +2. Mateus Rodrigues Martins (martinsrmateus@gmail.com) + +3. Juan Jose Quiroz Omana (juanjose.quirozomana@manchester.ac.uk) + - Added the joint_types member, and the following methods: + _check_joint_types(), and {set,get}_joint_{type, types}. +*/ + +#include +#include +#include + +namespace DQ_robotics +{ + +/** + * @brief Abstract class that defines serial manipulators. + * + * DQ_SerialManipulator extends DQ_Kinematics with the common operations of + * fixed-base and mobile-base serial chains. Subclasses implement the raw + * forward kinematics and raw Jacobians for a specific parameterization, + * while this class applies the reference frame and optional end-effector + * rigid transformation. + * + * @note The raw_* methods with = 0 are pure virtual and must be implemented + * by subclasses. The overloads without an explicit link index are concrete + * wrappers that default to the last link. + * + * @see DQ_Kinematics, DQ_SerialManipulatorDH, DQ_SerialManipulatorMDH + */ +class DQ_SerialManipulator: public DQ_Kinematics +{ +protected: + /** @brief Constant rigid transformation from the last link to the end effector. */ + DQ curr_effector_; + /** @brief Actuation type associated with each joint of the chain. */ + std::vector joint_types_; + /** + * @brief Constructs a serial manipulator with the given number of degrees of freedom. + * + * @param dofs Dimension of the configuration space. + */ + DQ_SerialManipulator(const int& dofs); + /** + * @brief Verifies whether the stored joint types are supported by the subclass. + * + * @throws std::runtime_error If at least one stored joint type is not supported. + */ + void _check_joint_types() const; +public: + /** + * @brief Returns the current end-effector rigid transformation. + * + * @return The constant rigid transformation appended to the last link. + */ + DQ get_effector() const; + /** + * @brief Sets the current end-effector rigid transformation. + * + * @param new_effector Constant rigid transformation from the last link to the tool frame. + * @return The stored end-effector transformation. + */ + DQ set_effector(const DQ& new_effector); + + /** @brief Returns the lower joint-position limits. */ + VectorXd get_lower_q_limit() const; + /** + * @brief Sets the lower joint-position limits. + * + * @param lower_q_limit Vector containing the lower position limits. + */ + void set_lower_q_limit(const VectorXd& lower_q_limit); + /** @brief Returns the lower joint-velocity limits. */ + VectorXd get_lower_q_dot_limit() const; + /** + * @brief Sets the lower joint-velocity limits. + * + * @param lower_q_dot_limit Vector containing the lower velocity limits. + */ + void set_lower_q_dot_limit(const VectorXd &lower_q_dot_limit); + /** @brief Returns the upper joint-position limits. */ + VectorXd get_upper_q_limit() const; + /** + * @brief Sets the upper joint-position limits. + * + * @param upper_q_limit Vector containing the upper position limits. + */ + void set_upper_q_limit(const VectorXd& upper_q_limit); + /** @brief Returns the upper joint-velocity limits. */ + VectorXd get_upper_q_dot_limit() const; + /** + * @brief Sets the upper joint-velocity limits. + * + * @param upper_q_dot_limit Vector containing the upper velocity limits. + */ + void set_upper_q_dot_limit(const VectorXd &upper_q_dot_limit); + + /** + * @brief Returns the actuation type of a given joint. + * + * @param ith_joint Joint index. + * @return The actuation type of the selected joint. + */ + DQ_JointType get_joint_type(const int& ith_joint) const; + /** + * @brief Returns the actuation types of all joints. + * + * @return Vector containing the actuation type of each joint. + */ + std::vector get_joint_types() const; + /** + * @brief Sets the actuation type of a given joint. + * + * @param joint_type Joint type to be assigned. + * @param ith_joint Joint index. + * @throws std::runtime_error If the resulting list of joint types is not supported. + */ + void set_joint_type(const DQ_JointType& joint_type, const int& ith_joint); + /** + * @brief Sets the actuation types of all joints. + * + * @param joint_types Vector containing the desired joint types. + * @throws std::runtime_error If at least one joint type is not supported. + */ + void set_joint_types(const std::vector& joint_types); + /** + * @brief Sets the actuation types of all joints from numeric values. + * + * Each entry is converted to a DQ_JointType according to the integer-based + * constructor of DQ_JointType. + * + * @param joint_types Numeric vector whose entries encode the desired joint types. + * @throws std::runtime_error If at least one joint type is invalid or not supported. + */ + void set_joint_types(const VectorXd& joint_types); + + //Virtual + /** + * @brief Computes the raw pose Jacobian up to the last link. + * + * This concrete overload delegates to raw_pose_jacobian(q_vec, get_dim_configuration_space() - 1). + * + * @param q_vec Vector containing the robot joint configurations. + * @return The raw pose Jacobian, without reference-frame or end-effector transformations. + */ + virtual MatrixXd raw_pose_jacobian(const VectorXd& q_vec) const; + /** + * @brief Computes the time derivative of the raw pose Jacobian up to the last link. + * + * This concrete overload delegates to raw_pose_jacobian_derivative(q, q_dot, get_dim_configuration_space() - 1). + * + * @param q Vector containing the robot joint configurations. + * @param q_dot Vector containing the robot joint velocities. + * @return The derivative of the raw pose Jacobian. + */ + virtual MatrixXd raw_pose_jacobian_derivative(const VectorXd& q, const VectorXd& q_dot) const; + /** + * @brief Computes the raw forward kinematics up to the last link. + * + * This concrete overload delegates to raw_fkm(q_vec, get_dim_configuration_space() - 1). + * + * @param q_vec Vector containing the robot joint configurations. + * @return The pose of the last link before applying the reference frame and the end effector. + */ + virtual DQ raw_fkm(const VectorXd& q_vec) const; + + //Pure virtual + /** + * @brief Computes the raw pose Jacobian up to a given link. + * + * This is a pure virtual interface contract and must be implemented by subclasses. + * + * @param q_vec Vector containing the robot joint configurations. + * @param to_ith_link Index of the last link to be accounted for in the computation. + * @return The raw pose Jacobian, without reference-frame or end-effector transformations. + */ + virtual MatrixXd raw_pose_jacobian(const VectorXd& q_vec, const int& to_ith_link) const = 0; + /** + * @brief Computes the time derivative of the raw pose Jacobian up to a given link. + * + * This is a pure virtual interface contract and must be implemented by subclasses. + * + * @param q Vector containing the robot joint configurations. + * @param q_dot Vector containing the robot joint velocities. + * @param to_ith_link Index of the last link to be accounted for in the computation. + * @return The derivative of the raw pose Jacobian. + */ + virtual MatrixXd raw_pose_jacobian_derivative(const VectorXd& q, const VectorXd& q_dot, const int& to_ith_link) const = 0; + /** + * @brief Computes the raw forward kinematics up to a given link. + * + * This is a pure virtual interface contract and must be implemented by subclasses. + * + * @param q_vec Vector containing the robot joint configurations. + * @param to_ith_link Index of the last link to be accounted for in the computation. + * @return The raw pose of the ith link, without reference-frame or end-effector transformations. + */ + virtual DQ raw_fkm(const VectorXd& q_vec, const int& to_ith_link) const = 0; + /** + * @brief Returns the joint types supported by the subclass. + * + * This is a pure virtual interface contract and must be implemented by subclasses. + * + * @return Vector containing the supported joint types. + */ + virtual std::vector get_supported_joint_types() const = 0; + + //Overrides from DQ_Kinematics + /** + * @brief Computes the forward kinematics of the end effector. + * + * This concrete override applies the reference frame and the stored + * end-effector rigid transformation. + * + * @param q_vec Vector containing the robot joint configurations. + * @return The pose of the end effector as a unit dual quaternion. + */ + virtual DQ fkm(const VectorXd& q_vec) const override; //Override from DQ_Kinematics + /** + * @brief Computes the forward kinematics up to a given link. + * + * The returned pose includes the reference frame. The stored end-effector + * transformation is applied only when @p to_ith_link is the last link. + * + * @param q_vec Vector containing the robot joint configurations. + * @param to_ith_link Index of the last link to be accounted for in the computation. + * @return The pose of the ith link as a unit dual quaternion. + */ + virtual DQ fkm(const VectorXd& q_vec, const int& to_ith_link) const override; //Override from DQ_Kinematics + + /** + * @brief Returns the dimension of the configuration space. + * + * @return Number of generalized coordinates of the serial manipulator. + */ + virtual int get_dim_configuration_space() const override; //Override from DQ_Kinematics + + /** + * @brief Computes the pose Jacobian up to a given link. + * + * The returned Jacobian includes the reference frame. The stored end-effector + * transformation is applied only when @p to_ith_link is the last link. + * + * @param q_vec Vector containing the robot joint configurations. + * @param to_ith_link Index of the last link to be accounted for in the computation. + * @return The pose Jacobian that satisfies vec8(pose_dot) = J * q_dot. + */ + virtual MatrixXd pose_jacobian(const VectorXd& q_vec, const int& to_ith_link) const override; //Override from DQ_Kinematics + /** + * @brief Computes the pose Jacobian of the end effector. + * + * @param q_vec Vector containing the robot joint configurations. + * @return The pose Jacobian that satisfies vec8(pose_dot) = J * q_dot. + */ + virtual MatrixXd pose_jacobian(const VectorXd& q_vec) const override; //Override from DQ_Kinematics + /** + * @brief Computes the time derivative of the pose Jacobian up to a given link. + * + * The returned Jacobian derivative includes the reference frame. The stored + * end-effector transformation is applied only when @p to_ith_link is the last link. + * + * @param q Vector containing the robot joint configurations. + * @param q_dot Vector containing the robot joint velocities. + * @param to_ith_link Index of the last link to be accounted for in the computation. + * @return The derivative of the pose Jacobian. + */ + virtual MatrixXd pose_jacobian_derivative(const VectorXd& q, const VectorXd& q_dot, const int& to_ith_link) const override; //Override from DQ_Kinematics + /** + * @brief Computes the time derivative of the pose Jacobian of the end effector. + * + * @param q Vector containing the robot joint configurations. + * @param q_dot Vector containing the robot joint velocities. + * @return The derivative of the pose Jacobian. + */ + virtual MatrixXd pose_jacobian_derivative(const VectorXd& q, const VectorXd& q_dot) const override; //Override from DQ_Kinematics + +}; + +} diff --git a/include/dqrobotics/robot_modeling/DQ_SerialManipulatorDH.h b/include/dqrobotics/robot_modeling/DQ_SerialManipulatorDH.h index 56e01d4..61ffaab 100644 --- a/include/dqrobotics/robot_modeling/DQ_SerialManipulatorDH.h +++ b/include/dqrobotics/robot_modeling/DQ_SerialManipulatorDH.h @@ -33,43 +33,157 @@ This file is part of DQ Robotics. namespace DQ_robotics { +/** + * @brief Concrete serial manipulator based on the standard Denavit-Hartenberg convention. + * + * The constructor expects a 5 x n matrix whose rows store theta, d, a, alpha, + * and the joint type of each link. Revolute joints use the theta row as joint + * offset, whereas prismatic joints use the d row as joint offset. + * + * @see DQ_SerialManipulator, DQ_SerialManipulatorMDH, DQ_ParameterDH + */ class DQ_SerialManipulatorDH: public DQ_SerialManipulator { protected: + /** @brief Matrix storing theta, d, a, alpha, and joint-type rows. */ MatrixXd dh_matrix_; + /** + * @brief Returns the dual quaternion term used in the pose-Jacobian derivative recursion. + * + * @param ith Link index. + * @return The dual quaternion term associated with the ith joint under the standard DH convention. + */ DQ _get_w(const int& ith) const; + /** + * @brief Converts the ith standard DH link transform into a dual quaternion. + * + * @param q Joint value associated with the ith joint. + * @param ith Link index. + * @return The dual quaternion representing the ith link transform. + */ DQ _dh2dq(const double& q, const int& ith) const; public: + /** + * @brief Returns one row of the stored DH matrix. + * + * @param parameter_type DH parameter to be retrieved. + * @return Vector containing the selected parameter for all joints. + * @throws std::runtime_error If @p parameter_type is not supported. + */ VectorXd get_parameters(const DQ_ParameterDH& parameter_type) const; + /** + * @brief Returns a DH parameter of a specific joint. + * + * @param parameter_type DH parameter to be retrieved. + * @param to_ith_link Joint index. + * @return Value of the selected parameter at the requested joint. + * @throws std::runtime_error If @p parameter_type is not supported. + */ double get_parameter(const DQ_ParameterDH& parameter_type, const int& to_ith_link) const; + /** + * @brief Replaces one row of the stored DH matrix. + * + * @param parameter_type DH parameter to be updated. + * @param vector_parameters Vector containing the new values for all joints. + */ void set_parameters(const DQ_ParameterDH& parameter_type, const VectorXd& vector_parameters); + /** + * @brief Sets a DH parameter of a specific joint. + * + * @param parameter_type DH parameter to be updated. + * @param to_ith_link Joint index. + * @param parameter New parameter value. + */ void set_parameter(const DQ_ParameterDH& parameter_type, const int& to_ith_link, const double& parameter); + /** + * @brief Returns the joint types supported by the standard DH implementation. + * + * @return Vector containing REVOLUTE and PRISMATIC. + */ std::vector get_supported_joint_types()const override; // Deprecated on 22.04, will be removed on the next release. - enum [[deprecated("Use DQ_JointType instead.")]] JOINT_TYPES{ JOINT_ROTATIONAL=0, JOINT_PRISMATIC }; - + /** @brief Deprecated joint-type constants kept for backward compatibility. */ + enum [[deprecated("Use DQ_JointType instead.")]] JOINT_TYPES{ JOINT_ROTATIONAL=0, /**< Revolute joint. */ JOINT_PRISMATIC /**< Prismatic joint. */ }; + + /** + * @brief Returns the theta row of the stored DH matrix. + * @return Vector containing all theta parameters. + * @deprecated Use get_parameters(DQ_ParameterDH::THETA) instead. + */ [[deprecated("Use get_parameters(DQ_ParameterDH::THETA) instead.")]] VectorXd get_thetas() const; + /** + * @brief Returns the d row of the stored DH matrix. + * @return Vector containing all d parameters. + * @deprecated Use get_parameters(DQ_ParameterDH::D) instead. + */ [[deprecated("Use get_parameters(DQ_ParameterDH::D) instead.")]] VectorXd get_ds() const; + /** + * @brief Returns the a row of the stored DH matrix. + * @return Vector containing all a parameters. + * @deprecated Use get_parameters(DQ_ParameterDH::A) instead. + */ [[deprecated("Use get_parameters(DQ_ParameterDH::A) instead.")]] VectorXd get_as() const; + /** + * @brief Returns the alpha row of the stored DH matrix. + * @return Vector containing all alpha parameters. + * @deprecated Use get_parameters(DQ_ParameterDH::ALPHA) instead. + */ [[deprecated("Use get_parameters(DQ_ParameterDH::ALPHA) instead.")]] VectorXd get_alphas() const; + /** + * @brief Returns the joint-type row of the stored DH matrix. + * @return Vector containing the encoded joint types. + * @deprecated Use get_joint_types() instead. + */ [[deprecated("Use get_joint_types() instead.")]] VectorXd get_types() const; + /** @brief Deleted default constructor. */ DQ_SerialManipulatorDH()=delete; + /** + * @brief Constructs a serial manipulator from a standard DH matrix. + * + * @param dh_matrix 5 x n matrix containing theta, d, a, alpha, and joint-type rows. + * @throws std::range_error If @p dh_matrix does not have exactly 5 rows. + */ DQ_SerialManipulatorDH(const MatrixXd& dh_matrix); + /** @brief Exposes the overload that computes the raw pose Jacobian up to the last link. */ using DQ_SerialManipulator::raw_pose_jacobian; + /** @brief Exposes the overload that computes the raw pose-Jacobian derivative up to the last link. */ using DQ_SerialManipulator::raw_pose_jacobian_derivative; + /** @brief Exposes the overload that computes the raw forward kinematics up to the last link. */ using DQ_SerialManipulator::raw_fkm; + /** + * @brief Computes the raw pose Jacobian under the standard DH convention. + * + * @param q_vec Vector containing the robot joint configurations. + * @param to_ith_link Index of the last link to be accounted for in the computation. + * @return The raw pose Jacobian up to the requested link. + */ MatrixXd raw_pose_jacobian(const VectorXd& q_vec, const int& to_ith_link) const override; + /** + * @brief Computes the time derivative of the raw pose Jacobian under the standard DH convention. + * + * @param q Vector containing the robot joint configurations. + * @param q_dot Vector containing the robot joint velocities. + * @param to_ith_link Index of the last link to be accounted for in the computation. + * @return The derivative of the raw pose Jacobian up to the requested link. + */ MatrixXd raw_pose_jacobian_derivative(const VectorXd& q, const VectorXd& q_dot, const int& to_ith_link) const override; + /** + * @brief Computes the raw forward kinematics under the standard DH convention. + * + * @param q_vec Vector containing the robot joint configurations. + * @param to_ith_link Index of the last link to be accounted for in the computation. + * @return The raw pose of the requested link. + */ DQ raw_fkm(const VectorXd &q_vec, const int &to_ith_link) const override; }; diff --git a/include/dqrobotics/robot_modeling/DQ_SerialManipulatorDenso.h b/include/dqrobotics/robot_modeling/DQ_SerialManipulatorDenso.h index d083f51..fd9aec9 100644 --- a/include/dqrobotics/robot_modeling/DQ_SerialManipulatorDenso.h +++ b/include/dqrobotics/robot_modeling/DQ_SerialManipulatorDenso.h @@ -31,32 +31,116 @@ This file is part of DQ Robotics. namespace DQ_robotics { +/** + * @brief Concrete serial manipulator that uses the DENSO kinematic convention. + * + * The constructor expects a 6 x n matrix whose rows store the convention + * parameters a, b, d, alpha, beta, and gamma for each link. The current + * implementation supports revolute joints only. + * + * @see DQ_SerialManipulator, DQ_JointType + */ class DQ_SerialManipulatorDenso: public DQ_SerialManipulator { protected: + /** @brief Matrix storing a, b, d, alpha, beta, and gamma rows. */ MatrixXd denso_matrix_; + /** + * @brief Converts the ith DENSO link transform into a dual quaternion. + * + * @param q Joint value associated with the ith joint. + * @param ith Link index. + * @return The dual quaternion representing the ith link transform. + */ DQ _denso2dh(const double& q, const int& ith) const; public: + /** + * @brief Returns the joint types supported by the DENSO implementation. + * + * @return Vector containing only REVOLUTE. + */ std::vector get_supported_joint_types() const override; // Deprecated on 22.04, will be removed on the next release. + /** + * @brief Returns the a row of the stored DENSO matrix. + * @return Vector containing all a parameters. + * @deprecated This accessor is kept for backward compatibility. + */ [[deprecated("Use ? instead.")]] VectorXd get_as() const; + /** + * @brief Returns the b row of the stored DENSO matrix. + * @return Vector containing all b parameters. + * @deprecated This accessor is kept for backward compatibility. + */ [[deprecated("Use ? instead.")]] VectorXd get_bs() const; + /** + * @brief Returns the d row of the stored DENSO matrix. + * @return Vector containing all d parameters. + * @deprecated This accessor is kept for backward compatibility. + */ [[deprecated("Use ? instead.")]] VectorXd get_ds() const; + /** + * @brief Returns the alpha row of the stored DENSO matrix. + * @return Vector containing all alpha parameters. + * @deprecated This accessor is kept for backward compatibility. + */ [[deprecated("Use ? instead.")]] VectorXd get_alphas() const; + /** + * @brief Returns the beta row of the stored DENSO matrix. + * @return Vector containing all beta parameters. + * @deprecated This accessor is kept for backward compatibility. + */ [[deprecated("Use ? instead.")]] VectorXd get_betas() const; + /** + * @brief Returns the gamma row of the stored DENSO matrix. + * @return Vector containing all gamma parameters. + * @deprecated This accessor is kept for backward compatibility. + */ [[deprecated("Use ? instead.")]] VectorXd get_gammas() const; + /** @brief Deleted default constructor. */ DQ_SerialManipulatorDenso()=delete; + /** + * @brief Constructs a serial manipulator from a DENSO-parameter matrix. + * + * @param denso_matrix 6 x n matrix containing a, b, d, alpha, beta, and gamma rows. + * @throws std::range_error If @p denso_matrix does not have exactly 6 rows. + */ DQ_SerialManipulatorDenso(const MatrixXd& denso_matrix); + /** @brief Exposes the overload that computes the raw pose Jacobian up to the last link. */ using DQ_SerialManipulator::raw_pose_jacobian; + /** @brief Exposes the overload that computes the raw pose-Jacobian derivative up to the last link. */ using DQ_SerialManipulator::raw_pose_jacobian_derivative; + /** @brief Exposes the overload that computes the raw forward kinematics up to the last link. */ using DQ_SerialManipulator::raw_fkm; + /** + * @brief Computes the raw pose Jacobian under the DENSO convention. + * + * @param q_vec Vector containing the robot joint configurations. + * @param to_ith_link Index of the last link to be accounted for in the computation. + * @return The raw pose Jacobian up to the requested link. + */ MatrixXd raw_pose_jacobian(const VectorXd& q_vec, const int& to_ith_link) const override; + /** + * @brief Computes the time derivative of the raw pose Jacobian under the DENSO convention. + * + * @param q Vector containing the robot joint configurations. + * @param q_dot Vector containing the robot joint velocities. + * @param to_ith_link Index of the last link to be accounted for in the computation. + * @return The derivative of the raw pose Jacobian up to the requested link. + */ MatrixXd raw_pose_jacobian_derivative(const VectorXd& q, const VectorXd& q_dot, const int& to_ith_link) const override; + /** + * @brief Computes the raw forward kinematics under the DENSO convention. + * + * @param q_vec Vector containing the robot joint configurations. + * @param to_ith_link Index of the last link to be accounted for in the computation. + * @return The raw pose of the requested link. + */ DQ raw_fkm(const VectorXd &q_vec, const int &to_ith_link) const override; }; diff --git a/include/dqrobotics/robot_modeling/DQ_SerialManipulatorMDH.h b/include/dqrobotics/robot_modeling/DQ_SerialManipulatorMDH.h index d48f77d..94c201c 100644 --- a/include/dqrobotics/robot_modeling/DQ_SerialManipulatorMDH.h +++ b/include/dqrobotics/robot_modeling/DQ_SerialManipulatorMDH.h @@ -32,43 +32,157 @@ This file is part of DQ Robotics. namespace DQ_robotics { +/** + * @brief Concrete serial manipulator based on the modified Denavit-Hartenberg convention. + * + * The constructor expects a 5 x n matrix whose rows store theta, d, a, alpha, + * and the joint type of each link. Revolute joints use the theta row as joint + * offset, whereas prismatic joints use the d row as joint offset. + * + * @see DQ_SerialManipulator, DQ_SerialManipulatorDH, DQ_ParameterDH + */ class DQ_SerialManipulatorMDH: public DQ_SerialManipulator { protected: + /** @brief Matrix storing theta, d, a, alpha, and joint-type rows. */ MatrixXd mdh_matrix_; + /** + * @brief Returns the dual quaternion term used in the pose-Jacobian derivative recursion. + * + * @param ith Link index. + * @return The dual quaternion term associated with the ith joint under the modified DH convention. + */ DQ _get_w(const int& ith) const; + /** + * @brief Converts the ith modified DH link transform into a dual quaternion. + * + * @param q Joint value associated with the ith joint. + * @param ith Link index. + * @return The dual quaternion representing the ith link transform. + */ DQ _mdh2dq(const double& q, const int& ith) const; public: + /** + * @brief Returns one row of the stored modified DH matrix. + * + * @param parameter_type Modified DH parameter to be retrieved. + * @return Vector containing the selected parameter for all joints. + * @throws std::runtime_error If @p parameter_type is not supported. + */ VectorXd get_parameters(const DQ_ParameterDH& parameter_type) const; + /** + * @brief Returns a modified DH parameter of a specific joint. + * + * @param parameter_type Modified DH parameter to be retrieved. + * @param to_ith_link Joint index. + * @return Value of the selected parameter at the requested joint. + * @throws std::runtime_error If @p parameter_type is not supported. + */ double get_parameter(const DQ_ParameterDH& parameter_type, const int& to_ith_link) const; + /** + * @brief Replaces one row of the stored modified DH matrix. + * + * @param parameter_type Modified DH parameter to be updated. + * @param vector_parameters Vector containing the new values for all joints. + */ void set_parameters(const DQ_ParameterDH& parameter_type, const VectorXd& vector_parameters); + /** + * @brief Sets a modified DH parameter of a specific joint. + * + * @param parameter_type Modified DH parameter to be updated. + * @param to_ith_link Joint index. + * @param parameter New parameter value. + */ void set_parameter(const DQ_ParameterDH& parameter_type, const int& to_ith_link, const double& parameter); + /** + * @brief Returns the joint types supported by the modified DH implementation. + * + * @return Vector containing REVOLUTE and PRISMATIC. + */ std::vector get_supported_joint_types()const override; // Deprecated on 22.04, will be removed on the next release. - enum [[deprecated("Use DQ_JointType instead.")]] JOINT_TYPES{ JOINT_ROTATIONAL=0, JOINT_PRISMATIC }; - + /** @brief Deprecated joint-type constants kept for backward compatibility. */ + enum [[deprecated("Use DQ_JointType instead.")]] JOINT_TYPES{ JOINT_ROTATIONAL=0, /**< Revolute joint. */ JOINT_PRISMATIC /**< Prismatic joint. */ }; + + /** + * @brief Returns the theta row of the stored modified DH matrix. + * @return Vector containing all theta parameters. + * @deprecated Use get_parameters(DQ_ParameterDH::THETA) instead. + */ [[deprecated("Use get_parameters(DQ_ParameterDH::THETA) instead.")]] VectorXd get_thetas() const; + /** + * @brief Returns the d row of the stored modified DH matrix. + * @return Vector containing all d parameters. + * @deprecated Use get_parameters(DQ_ParameterDH::D) instead. + */ [[deprecated("Use get_parameters(DQ_ParameterDH::D) instead.")]] VectorXd get_ds() const; + /** + * @brief Returns the a row of the stored modified DH matrix. + * @return Vector containing all a parameters. + * @deprecated Use get_parameters(DQ_ParameterDH::A) instead. + */ [[deprecated("Use get_parameters(DQ_ParameterDH::A) instead.")]] VectorXd get_as() const; + /** + * @brief Returns the alpha row of the stored modified DH matrix. + * @return Vector containing all alpha parameters. + * @deprecated Use get_parameters(DQ_ParameterDH::ALPHA) instead. + */ [[deprecated("Use get_parameters(DQ_ParameterDH::ALPHA) instead.")]] VectorXd get_alphas() const; + /** + * @brief Returns the joint-type row of the stored modified DH matrix. + * @return Vector containing the encoded joint types. + * @deprecated Use get_joint_types() instead. + */ [[deprecated("Use get_joint_types() instead.")]] VectorXd get_types() const; + /** @brief Deleted default constructor. */ DQ_SerialManipulatorMDH()=delete; + /** + * @brief Constructs a serial manipulator from a modified DH matrix. + * + * @param mdh_matrix 5 x n matrix containing theta, d, a, alpha, and joint-type rows. + * @throws std::range_error If @p mdh_matrix does not have exactly 5 rows. + */ DQ_SerialManipulatorMDH(const MatrixXd& mdh_matrix); + /** @brief Exposes the overload that computes the raw pose Jacobian up to the last link. */ using DQ_SerialManipulator::raw_pose_jacobian; + /** @brief Exposes the overload that computes the raw pose-Jacobian derivative up to the last link. */ using DQ_SerialManipulator::raw_pose_jacobian_derivative; + /** @brief Exposes the overload that computes the raw forward kinematics up to the last link. */ using DQ_SerialManipulator::raw_fkm; + /** + * @brief Computes the raw pose Jacobian under the modified DH convention. + * + * @param q_vec Vector containing the robot joint configurations. + * @param to_ith_link Index of the last link to be accounted for in the computation. + * @return The raw pose Jacobian up to the requested link. + */ MatrixXd raw_pose_jacobian(const VectorXd& q_vec, const int& to_ith_link) const override; + /** + * @brief Computes the time derivative of the raw pose Jacobian under the modified DH convention. + * + * @param q Vector containing the robot joint configurations. + * @param q_dot Vector containing the robot joint velocities. + * @param to_ith_link Index of the last link to be accounted for in the computation. + * @return The derivative of the raw pose Jacobian up to the requested link. + */ MatrixXd raw_pose_jacobian_derivative(const VectorXd& q, const VectorXd& q_dot, const int& to_ith_link) const override; + /** + * @brief Computes the raw forward kinematics under the modified DH convention. + * + * @param q_vec Vector containing the robot joint configurations. + * @param to_ith_link Index of the last link to be accounted for in the computation. + * @return The raw pose of the requested link. + */ DQ raw_fkm(const VectorXd &q_vec, const int &to_ith_link) const override; }; diff --git a/include/dqrobotics/robot_modeling/DQ_SerialWholeBody.h b/include/dqrobotics/robot_modeling/DQ_SerialWholeBody.h index ad4cb35..fd29250 100644 --- a/include/dqrobotics/robot_modeling/DQ_SerialWholeBody.h +++ b/include/dqrobotics/robot_modeling/DQ_SerialWholeBody.h @@ -33,44 +33,211 @@ This file is part of DQ Robotics. namespace DQ_robotics { +/** + * @brief Robot model composed of multiple serially coupled kinematic chains. + * + * DQ_SerialWholeBody concatenates several DQ_Kinematics objects and exposes a + * single combined link index across the whole serial composition. Methods with + * explicit chain and link indices are provided to address a particular subchain. + * + * @note The constructor currently accepts the string "standard". The string + * "reversed" is recognized by the implementation but is not implemented. + * + * @see DQ_Kinematics, DQ_WholeBody + */ class DQ_SerialWholeBody : public DQ_Kinematics { protected: + /** @brief Ordered list of kinematic chains that form the serial whole-body model. */ std::vector> chain_; + /** + * @brief Checks whether a chain index is valid. + * + * @param to_ith_chain Chain index. + * @throws std::runtime_error If @p to_ith_chain is outside the valid range. + */ void _check_to_ith_chain(const int& to_ith_chain) const; + /** + * @brief Checks whether a link index is valid inside a given chain. + * + * @param to_ith_chain Chain index. + * @param to_jth_link Link index inside the selected chain. + * @throws std::runtime_error If either index is outside the valid range. + */ void _check_to_jth_link_of_ith_chain(const int& to_ith_chain, const int& to_jth_link) const; public: + /** @brief Deleted default constructor. */ DQ_SerialWholeBody()=delete; + /** + * @brief Constructs a serial whole-body model from its first chain. + * + * @param robot Shared pointer to the first chain. + * @param type Composition type. The current implementation accepts "standard". + * @throws std::runtime_error If @p type is invalid or corresponds to an unimplemented mode. + */ DQ_SerialWholeBody(std::shared_ptr robot, const std::string type=std::string("standard")); + /** + * @brief Appends a new chain to the end of the serial whole-body model. + * + * @param robot Shared pointer to the chain to be appended. + */ void add(std::shared_ptr robot); + /** + * @brief Computes the raw forward kinematics up to a specific link inside a specific chain. + * + * @param q Combined configuration vector of the serial whole-body model. + * @param to_ith_chain Index of the last chain to be accounted for. + * @param to_jth_link Link index inside the last chain. + * @return Pose obtained by composing the chains up to the requested chain and link, without the reference frame. + */ DQ raw_fkm_by_chain(const VectorXd& q, const int& to_ith_chain, const int& to_jth_link) const; + /** + * @brief Computes the raw forward kinematics up to the end of a specific chain. + * + * @param q Combined configuration vector of the serial whole-body model. + * @param to_ith_chain Index of the last chain to be accounted for. + * @return Pose obtained by composing the chains up to the end of @p to_ith_chain, without the reference frame. + */ DQ raw_fkm_by_chain(const VectorXd& q, const int& to_ith_chain) const; + /** + * @brief Maps a combined link index to a pair of chain and local-link indices. + * + * @param to_ith_link Link index in the combined serial whole-body model. + * @return Tuple containing the chain index and the link index inside that chain. + * @throws std::runtime_error If the mapping cannot be determined. + */ std::tuple get_chain_and_link_from_index(const int& to_ith_link) const; + /** + * @brief Computes the raw forward kinematics of the complete serial whole-body model. + * + * @param q Combined configuration vector of the serial whole-body model. + * @return Pose of the last link without the reference frame. + */ DQ raw_fkm(const VectorXd& q) const; + /** + * @brief Computes the raw forward kinematics up to a combined link index. + * + * @param q Combined configuration vector of the serial whole-body model. + * @param to_ith_link Link index in the combined serial whole-body model. + * @return Pose of the requested link without the reference frame. + */ DQ raw_fkm(const VectorXd& q, const int& to_ith_link) const; + /** + * @brief Sets an end-effector rigid transformation on the last chain. + * + * This method is intended for serial whole-body chains whose last element is a + * serial manipulator. + * + * @param effector Constant rigid transformation from the last link to the tool frame. + */ void set_effector(const DQ& effector); + /** + * @brief Returns a raw pointer to one of the stored chains. + * + * @param to_ith_chain Chain index. + * @return Raw pointer to the selected chain. + */ DQ_Kinematics* get_chain(const int& to_ith_chain); + /** + * @brief Returns a copy of the selected chain as a DQ_SerialManipulatorDH. + * + * This method expects the selected chain to have dynamic type DQ_SerialManipulatorDH. + * + * @param to_ith_chain Chain index. + * @return Copy of the selected chain as a DQ_SerialManipulatorDH. + */ DQ_SerialManipulatorDH get_chain_as_serial_manipulator_dh(const int& to_ith_chain) const; + /** + * @brief Returns a copy of the selected chain as a DQ_HolonomicBase. + * + * This method expects the selected chain to have dynamic type DQ_HolonomicBase. + * + * @param to_ith_chain Chain index. + * @return Copy of the selected chain as a DQ_HolonomicBase. + */ DQ_HolonomicBase get_chain_as_holonomic_base(const int& to_ith_chain) const; + /** + * @brief Computes the raw pose Jacobian up to a specific link inside a specific chain. + * + * @param q Combined configuration vector of the serial whole-body model. + * @param to_ith_chain Index of the last chain to be accounted for. + * @param to_jth_link Link index inside the last chain. + * @return Raw pose Jacobian up to the requested chain and link. + */ MatrixXd raw_pose_jacobian_by_chain(const VectorXd& q, const int& to_ith_chain, const int& to_jth_link) const; + /** + * @brief Computes the raw time derivative of the pose Jacobian up to a specific link inside a specific chain. + * + * @param q Combined configuration vector of the serial whole-body model. + * @param q_dot Combined configuration-velocity vector. + * @param to_ith_chain Index of the last chain to be accounted for. + * @param to_jth_link Link index inside the last chain. + * @return Raw pose-Jacobian derivative up to the requested chain and link. + * @throws std::runtime_error Always, because this method is not implemented. + */ MatrixXd raw_pose_jacobian_derivative_by_chain(const VectorXd& q, const VectorXd& q_dot, const int& to_ith_chain, const int& to_jth_link) const; //To be implemented. //Abstract methods' implementation + /** + * @brief Computes the forward kinematics of the complete serial whole-body model. + * + * @param q Combined configuration vector of the serial whole-body model. + * @return Serial whole-body pose including the reference frame. + */ DQ fkm(const VectorXd& q) const override; + /** + * @brief Computes the forward kinematics up to a combined link index. + * + * This overload receives the combined configuration vector of the serial whole-body model + * and stops the computation at the link indexed by @p to_ith_link. + * + * @param to_ith_link Link index in the combined serial whole-body model. + * @return Pose of the requested link including the reference frame. + */ DQ fkm(const VectorXd&, const int& to_ith_link) const override; + /** + * @brief Computes the pose Jacobian up to a combined link index. + * + * @param q Combined configuration vector of the serial whole-body model. + * @param to_ith_link Link index in the combined serial whole-body model. + * @return Pose Jacobian up to the requested link. + */ MatrixXd pose_jacobian(const VectorXd& q, const int& to_ith_link) const override; + /** + * @brief Computes the pose Jacobian of the complete serial whole-body model. + * + * @param q Combined configuration vector of the serial whole-body model. + * @return Pose Jacobian of the complete model. + */ MatrixXd pose_jacobian(const VectorXd& q) const override; + /** + * @brief Computes the time derivative of the pose Jacobian up to a combined link index. + * + * @param q Combined configuration vector of the serial whole-body model. + * @param q_dot Combined configuration-velocity vector. + * @param to_ith_link Link index in the combined serial whole-body model. + * @return Pose-Jacobian derivative up to the requested link. + * @throws std::runtime_error Always, because the underlying derivative computation is not implemented. + */ MatrixXd pose_jacobian_derivative(const VectorXd& q, const VectorXd& q_dot, const int& to_ith_link) const override; //To be implemented. + /** + * @brief Computes the time derivative of the pose Jacobian of the complete serial whole-body model. + * + * @param q Combined configuration vector of the serial whole-body model. + * @param q_dot Combined configuration-velocity vector. + * @return Pose-Jacobian derivative of the complete model. + * @throws std::runtime_error Always, because the underlying derivative computation is not implemented. + */ MatrixXd pose_jacobian_derivative (const VectorXd& q, const VectorXd& q_dot) const override; //To be implemented. }; diff --git a/include/dqrobotics/robot_modeling/DQ_WholeBody.h b/include/dqrobotics/robot_modeling/DQ_WholeBody.h index 68ee02b..6b56120 100644 --- a/include/dqrobotics/robot_modeling/DQ_WholeBody.h +++ b/include/dqrobotics/robot_modeling/DQ_WholeBody.h @@ -33,31 +33,151 @@ This file is part of DQ Robotics. namespace DQ_robotics { +/** + * @brief Robot model composed of multiple kinematic chains connected in series. + * + * DQ_WholeBody concatenates several DQ_Kinematics objects and treats each one + * as a whole subchain. The methods that take an index operate at the subchain + * level, that is, they stop at the specified chain instead of at an individual link. + * + * @see DQ_Kinematics, DQ_SerialWholeBody + */ class DQ_WholeBody : public DQ_Kinematics { protected: + /** @brief Ordered list of kinematic chains that form the whole-body model. */ std::vector> chain_; + /** + * @brief Checks whether a chain index is valid. + * + * @param to_ith_chain Chain index. + * @throws std::runtime_error If @p to_ith_chain is outside the valid range. + */ void _check_to_ith_chain(const int& to_ith_chain) const; public: + /** @brief Deleted default constructor. */ DQ_WholeBody()=delete; + /** + * @brief Constructs a whole-body model from its first chain. + * + * @param robot Shared pointer to the first chain. + */ DQ_WholeBody(std::shared_ptr robot); + /** + * @brief Appends a new chain to the end of the whole-body model. + * + * @param robot Shared pointer to the chain to be appended. + */ void add(std::shared_ptr robot); + /** + * @brief Computes the raw forward kinematics up to a given chain. + * + * The returned pose does not include the reference frame. + * + * @param q Combined configuration vector of the whole-body model. + * @param to_ith_chain Index of the last chain to be accounted for. + * @return Pose obtained by composing the forward kinematics of the chains up to @p to_ith_chain. + * @throws std::runtime_error If @p to_ith_chain is outside the valid range. + */ DQ raw_fkm(const VectorXd& q, const int& to_ith_chain) const; + /** + * @brief Computes the raw forward kinematics of the complete whole-body model. + * + * @param q Combined configuration vector of the whole-body model. + * @return Pose of the last chain without the reference frame. + */ DQ raw_fkm(const VectorXd& q) const; + /** + * @brief Sets an end-effector rigid transformation on the last chain. + * + * This method is intended for whole-body chains whose last element is a + * serial manipulator. + * + * @param effector Constant rigid transformation from the last link to the tool frame. + */ void set_effector(const DQ& effector); + + /** + * @brief Returns a raw pointer to one of the stored chains. + * + * @param to_ith_chain Chain index. + * @return Raw pointer to the selected chain. + * @throws std::runtime_error If @p to_ith_chain is outside the valid range. + */ DQ_Kinematics* get_chain(const int& to_ith_chain); + /** + * @brief Returns a copy of the selected chain as a DQ_SerialManipulatorDH. + * + * This method expects the selected chain to have dynamic type DQ_SerialManipulatorDH. + * + * @param to_ith_chain Chain index. + * @return Copy of the selected chain as a DQ_SerialManipulatorDH. + */ DQ_SerialManipulatorDH get_chain_as_serial_manipulator_dh(const int& to_ith_chain) const; + /** + * @brief Returns a copy of the selected chain as a DQ_HolonomicBase. + * + * This method expects the selected chain to have dynamic type DQ_HolonomicBase. + * + * @param to_ith_chain Chain index. + * @return Copy of the selected chain as a DQ_HolonomicBase. + */ DQ_HolonomicBase get_chain_as_holonomic_base(const int& to_ith_chain) const; //Abstract methods' implementation + /** + * @brief Computes the forward kinematics of the complete whole-body model. + * + * @param q Combined configuration vector of the whole-body model. + * @return Whole-body pose including the reference frame. + */ DQ fkm(const VectorXd& q) const override; + /** + * @brief Computes the forward kinematics up to a given chain. + * + * This overload receives the combined configuration vector of the whole-body model + * and stops the computation at the chain indexed by @p to_chain. + * + * @param to_chain Index of the last chain to be accounted for. + * @return Whole-body pose up to the requested chain, including the reference frame. + */ DQ fkm(const VectorXd&, const int& to_chain) const override; + /** + * @brief Computes the pose Jacobian up to a given chain. + * + * @param q Combined configuration vector of the whole-body model. + * @param to_ith_chain Index of the last chain to be accounted for. + * @return Whole-body pose Jacobian up to the requested chain. + */ MatrixXd pose_jacobian(const VectorXd& q, const int& to_ith_chain) const override; + /** + * @brief Computes the pose Jacobian of the complete whole-body model. + * + * @param q Combined configuration vector of the whole-body model. + * @return Whole-body pose Jacobian. + */ MatrixXd pose_jacobian(const VectorXd& q) const override; + /** + * @brief Computes the time derivative of the pose Jacobian. + * + * @param q Combined configuration vector of the whole-body model. + * @param q_dot Combined configuration-velocity vector. + * @param to_ith_link Index parameter forwarded by the current interface. + * @return The pose-Jacobian derivative. + * @throws std::runtime_error Always, because this method is not implemented. + */ MatrixXd pose_jacobian_derivative(const VectorXd& q, const VectorXd& q_dot, const int& to_ith_link) const override; //To be implemented. + /** + * @brief Computes the time derivative of the pose Jacobian of the complete whole-body model. + * + * @param q Combined configuration vector of the whole-body model. + * @param q_dot Combined configuration-velocity vector. + * @return The pose-Jacobian derivative. + * @throws std::runtime_error Always, because this method is not implemented. + */ MatrixXd pose_jacobian_derivative (const VectorXd& q, const VectorXd& q_dot) const override; //To be implemented. }; diff --git a/include/dqrobotics/robots/Ax18ManipulatorRobot.h b/include/dqrobotics/robots/Ax18ManipulatorRobot.h index 36bef60..3e43203 100644 --- a/include/dqrobotics/robots/Ax18ManipulatorRobot.h +++ b/include/dqrobotics/robots/Ax18ManipulatorRobot.h @@ -27,9 +27,21 @@ This file is part of DQ Robotics. namespace DQ_robotics{ +/** + * @brief Provides the kinematic model of the AX-18 manipulator arm. + * + * This class exposes a ready-to-use model of the AX-18 arm based on the + * Denavit-Hartenberg parameters used in the MATLAB version of DQ Robotics. + */ class Ax18ManipulatorRobot { public: + /** + * @brief Returns the kinematic model of the AX-18 manipulator arm. + * @return A DQ_SerialManipulatorDH instance representing the robot. + * @note The model is described using the standard Denavit-Hartenberg + * convention. + */ static DQ_SerialManipulatorDH kinematics(); }; diff --git a/include/dqrobotics/robots/BarrettWamArmRobot.h b/include/dqrobotics/robots/BarrettWamArmRobot.h index f9f67e5..c65edd5 100644 --- a/include/dqrobotics/robots/BarrettWamArmRobot.h +++ b/include/dqrobotics/robots/BarrettWamArmRobot.h @@ -28,9 +28,21 @@ This file is part of DQ Robotics. namespace DQ_robotics { +/** + * @brief Provides the kinematic model of the Barrett WAM arm robot manipulator. + * + * This class exposes a ready-to-use model of the Barrett WAM arm based on the + * Denavit-Hartenberg parameters used in the MATLAB version of DQ Robotics. + */ class BarrettWamArmRobot { public: + /** + * @brief Returns the kinematic model of the Barrett WAM arm robot manipulator. + * @return A DQ_SerialManipulatorDH instance representing the robot. + * @note The model is described using the standard Denavit-Hartenberg + * convention. + */ static DQ_SerialManipulatorDH kinematics(); }; diff --git a/include/dqrobotics/robots/ComauSmartSixRobot.h b/include/dqrobotics/robots/ComauSmartSixRobot.h index 67fcf89..05b55b3 100644 --- a/include/dqrobotics/robots/ComauSmartSixRobot.h +++ b/include/dqrobotics/robots/ComauSmartSixRobot.h @@ -28,9 +28,21 @@ This file is part of DQ Robotics. namespace DQ_robotics { +/** + * @brief Provides the kinematic model of the COMAU SmartSiX robot manipulator. + * + * This class exposes a ready-to-use model of the COMAU SmartSiX robot based on + * the Denavit-Hartenberg parameters used in the MATLAB version of DQ Robotics. + */ class ComauSmartSixRobot { public: + /** + * @brief Returns the kinematic model of the COMAU SmartSiX robot manipulator. + * @return A DQ_SerialManipulatorDH instance representing the robot. + * @note The MATLAB reference model is described using the modified + * Denavit-Hartenberg convention. + */ static DQ_SerialManipulatorDH kinematics(); }; } diff --git a/include/dqrobotics/robots/FrankaEmikaPandaRobot.h b/include/dqrobotics/robots/FrankaEmikaPandaRobot.h index efc9b2d..4f6805a 100644 --- a/include/dqrobotics/robots/FrankaEmikaPandaRobot.h +++ b/include/dqrobotics/robots/FrankaEmikaPandaRobot.h @@ -21,13 +21,25 @@ This file is part of DQ Robotics. namespace DQ_robotics { +/** + * @brief Provides the kinematic model of the Franka Emika Panda robot manipulator. + * + * This class exposes a ready-to-use model of the Franka Emika Panda arm using + * the geometric data encoded in the library implementation. + */ class FrankaEmikaPandaRobot { public: + /** + * @brief Returns the kinematic model of the Franka Emika Panda robot, + * described using the modified Denavit-Hartenberg convention. + * @return A DQ_SerialManipulatorMDH instance representing the robot. + * @note The implementation also sets the manufacturer base and flange + * offsets, as well as the joint position and velocity limits. + */ static DQ_SerialManipulatorMDH kinematics(); //static DQ_SerialManipulatorMDH dynamics(); To be implemented }; } - diff --git a/include/dqrobotics/robots/KukaLw4Robot.h b/include/dqrobotics/robots/KukaLw4Robot.h index 951ebdf..04080ab 100644 --- a/include/dqrobotics/robots/KukaLw4Robot.h +++ b/include/dqrobotics/robots/KukaLw4Robot.h @@ -28,9 +28,21 @@ This file is part of DQ Robotics. namespace DQ_robotics { +/** + * @brief Provides the kinematic model of the KUKA LWR4 robot manipulator. + * + * This class exposes a ready-to-use model of the KUKA LWR4 arm based on the + * Denavit-Hartenberg parameters used in the MATLAB version of DQ Robotics. + */ class KukaLw4Robot { public: + /** + * @brief Returns the kinematic model of the KUKA LWR4 robot manipulator. + * @return A DQ_SerialManipulatorDH instance representing the robot. + * @note The model is described using the standard Denavit-Hartenberg + * convention. + */ static DQ_SerialManipulatorDH kinematics(); }; diff --git a/include/dqrobotics/robots/KukaYoubotRobot.h b/include/dqrobotics/robots/KukaYoubotRobot.h index ff35f77..7683f00 100644 --- a/include/dqrobotics/robots/KukaYoubotRobot.h +++ b/include/dqrobotics/robots/KukaYoubotRobot.h @@ -28,9 +28,23 @@ This file is part of DQ Robotics. namespace DQ_robotics { +/** + * @brief Provides the whole-body kinematic model of the KUKA youBot mobile manipulator. + * + * The model is composed of a holonomic mobile base serially coupled to the + * robot's 5-DOF arm, following the MATLAB version of DQ Robotics. + */ class KukaYoubotRobot { public: + /** + * @brief Returns the whole-body kinematic model of the KUKA youBot mobile manipulator. + * @return A DQ_SerialWholeBody instance representing the holonomic base and + * the serial arm. + * @note The arm parameters are described using the standard + * Denavit-Hartenberg convention, and the geometric dimensions follow KUKA's + * technical documentation. + */ static DQ_SerialWholeBody kinematics(); }; diff --git a/include/dqrobotics/solvers/DQ_QuadraticProgrammingSolver.h b/include/dqrobotics/solvers/DQ_QuadraticProgrammingSolver.h index 4dd389b..a7198b3 100644 --- a/include/dqrobotics/solvers/DQ_QuadraticProgrammingSolver.h +++ b/include/dqrobotics/solvers/DQ_QuadraticProgrammingSolver.h @@ -27,13 +27,40 @@ using namespace Eigen; namespace DQ_robotics { +/** + * @brief Abstract interface to quadratic-programming solvers used by DQ Robotics controllers. + * + * Concrete implementations solve optimization problems of the form + * min 0.5*u'*H*u + f'*u subject to A*u <= b and Aeq*u = beq. + * + * @see DQ_QuadraticProgrammingController + */ class DQ_QuadraticProgrammingSolver { protected: + /** + * @brief Default constructor for solver interfaces. + */ DQ_QuadraticProgrammingSolver() = default; public: + /** + * @brief Virtual destructor. + */ virtual ~DQ_QuadraticProgrammingSolver() = default; + /** + * @brief Solves a quadratic program. + * + * Pure virtual interface contract implemented by concrete quadratic-programming solvers. + * + * @param H Symmetric matrix of the quadratic term. + * @param f Vector of the linear term. + * @param A Matrix of inequality constraints. + * @param b Vector of inequality-constraint bounds. + * @param Aeq Matrix of equality constraints. + * @param beq Vector of equality-constraint bounds. + * @return The optimal decision vector. + */ virtual VectorXd solve_quadratic_program(const MatrixXd& H, const VectorXd& f, const MatrixXd& A, const VectorXd& b, const MatrixXd& Aeq, const VectorXd& beq)=0; }; } diff --git a/include/dqrobotics/utils/DQ_Constants.h b/include/dqrobotics/utils/DQ_Constants.h index f85b32c..353d49f 100644 --- a/include/dqrobotics/utils/DQ_Constants.h +++ b/include/dqrobotics/utils/DQ_Constants.h @@ -26,6 +26,12 @@ This file is part of DQ Robotics. namespace DQ_robotics { +/** + * @brief The circular constant \f$\pi\f$. + * + * This constant exposes the value of `M_PI` inside the DQ Robotics namespace + * so angle-conversion routines use a consistent definition. + */ constexpr double pi = M_PI; } diff --git a/include/dqrobotics/utils/DQ_Geometry.h b/include/dqrobotics/utils/DQ_Geometry.h index 9fb9a97..0c2f0f9 100644 --- a/include/dqrobotics/utils/DQ_Geometry.h +++ b/include/dqrobotics/utils/DQ_Geometry.h @@ -28,28 +28,151 @@ This file is part of DQ Robotics. namespace DQ_robotics { +/** + * @brief Provides geometric operations for points, lines, planes, and line segments. + * + * This class groups static methods that evaluate distances, angles, projections, + * and closest-point relations directly from the dual-quaternion geometric + * primitives used throughout DQ Robotics. + */ class DQ_Geometry { public: + /** + * @brief Computes the squared distance between two points represented as pure quaternions. + * + * @param point1 The pure quaternion representing the first point. + * @param point2 The pure quaternion representing the second point. + * @return The squared Euclidean distance between the two points. + * @throws std::range_error If either input is not a pure quaternion. + * @see point_to_line_squared_distance, point_to_plane_distance + */ static double point_to_point_squared_distance(const DQ& point1, const DQ& point2); + /** + * @brief Computes the squared distance between a point and a line. + * + * The point must be given as a pure quaternion and the line as a unit Plucker + * line represented by a pure dual quaternion. + * + * @param point The pure quaternion representing the point. + * @param line The dual quaternion representing the line. + * @return The squared Euclidean distance between the point and the line. + * @throws std::range_error If `point` is not a pure quaternion or `line` is not a line. + * @see point_to_point_squared_distance, point_projected_in_line + */ static double point_to_line_squared_distance(const DQ& point, const DQ& line); + /** + * @brief Computes the signed distance between a point and a plane. + * + * The point must be represented as a pure quaternion, and the plane must be + * represented as a unit dual quaternion with pure primary part and real dual + * part. + * + * @param point The pure quaternion representing the point. + * @param plane The dual quaternion representing the plane. + * @return The signed distance from the point to the plane. + * @note The sign is determined by the orientation of the plane normal. + * @throws std::range_error If `point` is not a pure quaternion or `plane` is not a plane. + * @see point_to_point_squared_distance, point_to_line_squared_distance + */ static double point_to_plane_distance(const DQ& point, const DQ& plane); + /** + * @brief Computes the squared distance between two lines. + * + * For non-parallel lines, the result follows the ratio between the norm of + * the dual part of their dot product and the norm of the primary part of + * their cross product. For parallel lines, the implementation falls back to + * the dual part of the line cross product. + * + * @param line1 The first dual quaternion line. + * @param line2 The second dual quaternion line. + * @return The squared Euclidean distance between the two lines. + * @note This method returns a squared distance, not the distance itself. + * @throws std::range_error If either input is not a line. + * @see line_to_line_angle, closest_points_between_lines + */ static double line_to_line_squared_distance(const DQ& line1, const DQ& line2); + /** + * @brief Computes the angle between two lines. + * + * The angle is obtained from the primary part of the dot product between the + * two dual-quaternion line representations. + * + * @param line1 The first dual quaternion line. + * @param line2 The second dual quaternion line. + * @return The angle between the two lines in radians. + * @throws std::range_error If either input is not a line. + * @see line_to_line_squared_distance + */ static double line_to_line_angle(const DQ& line1, const DQ& line2); + /** + * @brief Projects a point onto a line. + * + * @param point The pure quaternion representing the point to be projected. + * @param line The dual quaternion representing the line. + * @return The orthogonal projection of the point onto the line as a pure quaternion. + * @see point_to_line_squared_distance, closest_points_between_lines + */ static DQ point_projected_in_line(const DQ& point, const DQ& line); + /** + * @brief Computes the closest points between two lines. + * + * The returned tuple contains one point on each line. The closed-form + * expression assumes that the supporting lines are not parallel. + * + * @param line1 The first dual quaternion line. + * @param line2 The second dual quaternion line. + * @return A tuple containing the closest point on `line1` and the closest point on `line2`. + * @note Parallel lines do not define a unique closest-point pair for this formulation. + * @throws std::runtime_error If either input is not a line. + * @see line_to_line_squared_distance, point_projected_in_line + */ static std::tuple closest_points_between_lines(const DQ& line1, const DQ& line2); + /** + * @brief Checks whether a line and two endpoints define a valid line segment. + * + * The method verifies that the supporting primitive is a line, that both + * endpoints are pure quaternions, and that both endpoints lie on the line up + * to the supplied threshold. + * + * @param line The dual quaternion representing the supporting line. + * @param line_point_1 The first endpoint of the segment. + * @param line_point_2 The second endpoint of the segment. + * @param threshold The tolerance used to test whether each endpoint lies on the line. + * @return `true` if the inputs define a valid line segment and `false` otherwise. + * @note The tolerance is applied only to the point-on-line consistency test. + * @see closest_points_between_line_segments, line_segment_to_line_segment_squared_distance + */ static bool is_line_segment(const DQ& line, const DQ& line_point_1, const DQ& line_point_2, const double& threshold=DQ_threshold); + /** + * @brief Computes the closest points between two line segments. + * + * The implementation evaluates the relevant line-line, line-endpoint, and + * endpoint-endpoint candidates and returns the pair that minimizes the + * squared distance. + * + * @param line_1 The first supporting line. + * @param line_1_point_1 The first endpoint of the first segment. + * @param line_1_point_2 The second endpoint of the first segment. + * @param line_2 The second supporting line. + * @param line_2_point_1 The first endpoint of the second segment. + * @param line_2_point_2 The second endpoint of the second segment. + * @return A tuple containing the closest point on the first segment and the closest point on the second segment. + * @note A unique closest-point pair is only well defined when the supporting lines are not parallel. + * @throws std::runtime_error If either input triple does not define a valid line segment or if no unique pair can be resolved. + * @see closest_points_between_lines, line_segment_to_line_segment_squared_distance + */ static std::tuple closest_points_between_line_segments(const DQ& line_1, const DQ& line_1_point_1, const DQ& line_1_point_2, @@ -57,6 +180,26 @@ class DQ_Geometry const DQ& line_2_point_1, const DQ& line_2_point_2); + /** + * @brief Computes the squared distance between two line segments. + * + * For non-parallel supporting lines, the result is the minimum squared + * distance over the same candidate set used by + * closest_points_between_line_segments(). For parallel supporting lines, the + * method returns the squared distance between the corresponding infinite + * lines. + * + * @param line_1 The first supporting line. + * @param line_1_point_1 The first endpoint of the first segment. + * @param line_1_point_2 The second endpoint of the first segment. + * @param line_2 The second supporting line. + * @param line_2_point_1 The first endpoint of the second segment. + * @param line_2_point_2 The second endpoint of the second segment. + * @return The squared Euclidean distance between the two line segments. + * @note This method returns a squared distance, not the distance itself. + * @throws std::runtime_error If either input triple does not define a valid line segment. + * @see closest_points_between_line_segments, line_to_line_squared_distance + */ static double line_segment_to_line_segment_squared_distance(const DQ& line_1, const DQ& line_1_point_1, const DQ& line_1_point_2, diff --git a/include/dqrobotics/utils/DQ_LinearAlgebra.h b/include/dqrobotics/utils/DQ_LinearAlgebra.h index 3fc0e52..67a3ef3 100644 --- a/include/dqrobotics/utils/DQ_LinearAlgebra.h +++ b/include/dqrobotics/utils/DQ_LinearAlgebra.h @@ -29,10 +29,43 @@ using namespace Eigen; namespace DQ_robotics { +/** + * @brief Computes the rank of a matrix using singular value decomposition. + * + * The tolerance matches the MATLAB-inspired rule used by this library, + * namely `max(rows, cols) * sigma_max * eps`, where `sigma_max` is the + * largest singular value of the matrix. + * + * @param matrix The input matrix. + * @return The number of singular values greater than the default tolerance. + * @see pinv, svd + */ int rank(const MatrixXd& matrix); +/** + * @brief Computes the Moore-Penrose pseudoinverse of a matrix. + * + * The pseudoinverse is obtained from a full singular value decomposition. + * Singular values smaller than the MATLAB-style tolerance + * `max(rows, cols) * sigma_max * eps` are treated as zero. + * + * @param matrix The input matrix. + * @return The pseudoinverse of the input matrix. + * @see rank, svd + */ MatrixXd pinv(const MatrixXd& matrix); +/** + * @brief Computes the singular value decomposition of a matrix. + * + * The returned tuple is ordered as `(U, S, V)` so the original matrix can be + * reconstructed as `matrix = U * S * V.adjoint()`. + * + * @param matrix The input matrix. + * @return A tuple containing the left singular vectors, the diagonal matrix of + * singular values, and the right singular vectors. + * @see pinv, rank + */ std::tuple svd(const MatrixXd& matrix); } diff --git a/include/dqrobotics/utils/DQ_Math.h b/include/dqrobotics/utils/DQ_Math.h index 603e6e1..90a246f 100644 --- a/include/dqrobotics/utils/DQ_Math.h +++ b/include/dqrobotics/utils/DQ_Math.h @@ -27,18 +27,46 @@ This file is part of DQ Robotics. namespace DQ_robotics { +/** + * @brief Converts an angle from degrees to radians. + * + * @param a The angle in degrees. + * @return The same angle expressed in radians. + * @see rad2deg + */ constexpr double deg2rad(const double& a) noexcept { return (a)*pi/(180.0); } +/** + * @brief Converts each component of a vector from degrees to radians. + * + * @param v The input vector in degrees. + * @return A vector whose entries are the corresponding values in radians. + * @see rad2deg + */ VectorXd deg2rad(const VectorXd& v); +/** + * @brief Converts an angle from radians to degrees. + * + * @param a The angle in radians. + * @return The same angle expressed in degrees. + * @see deg2rad + */ constexpr double rad2deg(const double& a) noexcept { return (a)*180.0/(pi); } +/** + * @brief Converts each component of a vector from radians to degrees. + * + * @param v The input vector in radians. + * @return A vector whose entries are the corresponding values in degrees. + * @see deg2rad + */ VectorXd rad2deg(const VectorXd& v); }