From 27acf5a1f50a6250ba56ca5459bf83e5a4ff3bec Mon Sep 17 00:00:00 2001 From: Frederico Afonso Date: Mon, 12 May 2025 17:28:16 +0100 Subject: [PATCH 1/3] [DQ] Deprecated the obsolete constructor with default double parameters, added the new default constructor that takes no arguments, and refactored for compliance. --- include/dqrobotics/DQ.h | 21 ++++++++--- src/DQ.cpp | 84 +++++++++++++++++++++++++++++++---------- 2 files changed, 81 insertions(+), 24 deletions(-) diff --git a/include/dqrobotics/DQ.h b/include/dqrobotics/DQ.h index 6dc03d0..7cf950b 100644 --- a/include/dqrobotics/DQ.h +++ b/include/dqrobotics/DQ.h @@ -39,6 +39,13 @@ This file is part of DQ Robotics. 4. Marcos da Silva Pereira (marcos.si.pereira@gmail.com) - Translated the Q4 and the Q8 methods from the MATLAB implementation in PR #56 (https://github.com/dqrobotics/cpp/pull/56). + +5. Frederico Fernandes Afonso Silva (frederico.silva@ieee.org) + - Deprecated the obsolete constructor with default double parameters, + added the new default constructor that takes no arguments, and refactored + for compliance. + [ffasilva committed on MM DD, 2025](COMMIT_NUMBER) + (LINK). */ #pragma once @@ -79,10 +86,14 @@ class DQ{ const static DQ E; //Constructors + DQ() noexcept; explicit DQ(VectorXd&& v); explicit DQ(const VectorXd& v); - explicit DQ(const double& q0=0.0, + [[deprecated("Consider explicitly initializing dual quaternions using the " + "operators DQ::i_, DQ::j_, DQ::k_, DQ::E_. Alternatively, " + "use the constructor DQ::DQ(const VectorXd& v) instead.")]] + explicit DQ(const double& q0, const double& q1=0.0, const double& q2=0.0, const double& q3=0.0, @@ -287,10 +298,10 @@ std::ostream& operator<<(std::ostream &os, const DQ& dq); Matrix C8(); 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 i_((Matrix() << 0,1,0,0,0,0,0,0).finished()); +const DQ j_((Matrix() << 0,0,1,0,0,0,0,0).finished()); +const DQ k_((Matrix() << 0,0,0,1,0,0,0,0).finished()); +const DQ E_((Matrix() << 0,0,0,0,1,0,0,0).finished()); /************************************************************************* ************** DUAL QUATERNIONS AND SCALAR OPERATOR TEMPLATES *********** diff --git a/src/DQ.cpp b/src/DQ.cpp index 7043a68..40d485d 100644 --- a/src/DQ.cpp +++ b/src/DQ.cpp @@ -39,6 +39,13 @@ This file is part of DQ Robotics. 4. Marcos da Silva Pereira (marcos.si.pereira@gmail.com) - Translated the Q4 and the Q8 methods from the MATLAB implementation in PR #56 (https://github.com/dqrobotics/cpp/pull/56). + +5. Frederico Fernandes Afonso Silva (frederico.silva@ieee.org) + - Deprecated the obsolete constructor with default double parameters, + added the new default constructor that takes no arguments, and refactored + for compliance. + [ffasilva committed on MM DD, 2025](COMMIT_NUMBER) + (LINK). */ #include @@ -50,10 +57,10 @@ This file is part of DQ Robotics. namespace DQ_robotics{ //To comply with MATLAB -const DQ DQ::i(0,1,0,0,0,0,0,0); -const DQ DQ::j(0,0,1,0,0,0,0,0); -const DQ DQ::k(0,0,0,1,0,0,0,0); -const DQ DQ::E(0,0,0,0,1,0,0,0); +const DQ DQ::i((Matrix() << 0,1,0,0,0,0,0,0).finished()); +const DQ DQ::j((Matrix() << 0,0,1,0,0,0,0,0).finished()); +const DQ DQ::k((Matrix() << 0,0,0,1,0,0,0,0).finished()); +const DQ DQ::E((Matrix() << 0,0,0,0,1,0,0,0).finished()); /**************************************************************** **************NAMESPACE ONLY FUNCTIONS*************************** @@ -423,6 +430,15 @@ double DQ::q_(const int a) const return q(a); } +/** + * @brief Returns a dual quaternion with all its coefficients equal to zero. + * @return A dual quaternion with all its coefficients equal to zero. + */ +DQ::DQ() noexcept +{ + q = VectorXd::Zero(8); +} + DQ::DQ(VectorXd &&v) { switch (v.size()) @@ -526,7 +542,7 @@ DQ::DQ(const double& q0,const double& q1,const double& q2,const double& q3,const * \sa DQ(double q0,double q1,double q2,double q3). */ DQ DQ::P() const{ - return DQ(q(0),q(1),q(2),q(3)); + return DQ((Matrix() << q(0),q(1),q(2),q(3),0,0,0,0).finished()); } @@ -539,7 +555,7 @@ DQ DQ::P() const{ * \sa DQ(double q0,double q1,double q2,double q3). */ DQ DQ::D() const{ - return DQ(q(4),q(5),q(6),q(7)); + return DQ((Matrix() << q(4),q(5),q(6),q(7),0,0,0,0).finished()); } @@ -548,7 +564,7 @@ DQ DQ::D() const{ * Actually this function does the same as Re() changing only the way of calling, which is DQ::Re(dq_object). */ DQ DQ::Re() const{ - return DQ(q(0),0,0,0,q(4),0,0,0); + return DQ((Matrix() << q(0),0,0,0,q(4),0,0,0).finished()); } /** @@ -560,7 +576,14 @@ DQ DQ::Re() const{ * \sa DQ(double q0,double q1,double q2,double q3,double q4,double q5,double q6,double q7). */ DQ DQ::Im() const{ - return DQ(0,q(1),q(2),q(3),0,q(5),q(6),q(7)); + return DQ((Matrix() << 0, + q(1), + q(2), + q(3), + 0, + q(5), + q(6), + q(7)).finished()); } /** @@ -572,7 +595,14 @@ DQ DQ::Im() const{ * \sa DQ(double q0,double q1,double q2,double q3,double q4,double q5,double q6,double q7). */ DQ DQ::conj() const{ - return DQ(q(0),-q(1),-q(2),-q(3),q(4),-q(5),-q(6),-q(7)); + return DQ((Matrix() << q(0), + -q(1), + -q(2), + -q(3), + q(4), + -q(5), + -q(6), + -q(7)).finished()); } /** @@ -626,13 +656,19 @@ DQ DQ::inv() const{ } //inverse calculation aux2 = aux * aux.conj(); //(dq norm)^2 - DQ inv((1/aux2.q(0)),0,0,0,(-aux2.q(4)/(aux2.q(0)*aux2.q(0))),0,0,0); + DQ inv((Matrix() << (1/aux2.q(0)), + 0, + 0, + 0, + (-aux2.q(4)/(aux2.q(0)*aux2.q(0))), + 0, + 0, + 0).finished()); inv = (aux.conj() * inv); return inv; } - /** * Returns a constant DQ object representing the translation part of the unit DQ object caller. * @@ -747,7 +783,14 @@ DQ DQ::log() const{ // log calculation DQ p = (0.5 * this->rotation_angle()) * this->rotation_axis(); //primary DQ d = 0.5 * this->translation(); //dual - DQ log(p.q(0),p.q(1),p.q(2),p.q(3),d.q(0),d.q(1),d.q(2),d.q(3)); + DQ log((Matrix() << p.q(0), + p.q(1), + p.q(2), + p.q(3), + d.q(0), + d.q(1), + d.q(2), + d.q(3)).finished()); return log; @@ -778,7 +821,7 @@ DQ DQ::exp() const{ if(phi != 0.0) prim = cos(phi) + (sin(phi)/phi)*this->P(); else - prim = DQ(1.0); + prim = DQ((Matrix() << 1,0,0,0,0,0,0,0).finished()); exp = ( prim + E_*this->D()*prim ); @@ -1378,16 +1421,19 @@ const DQ operator *(const DQ& dq1, const DQ& dq2) noexcept const double& q6 = dq1.q(0)*qd2_d.q(2) - dq1.q(1)*qd2_d.q(3) + dq1.q(2)*qd2_d.q(0) + dq1.q(3)*qd2_d.q(1); const double& q7 = dq1.q(0)*qd2_d.q(3) + dq1.q(1)*qd2_d.q(2) - dq1.q(2)*qd2_d.q(1) + dq1.q(3)*qd2_d.q(0); - return DQ( + return DQ((Matrix() << dq1.q(0)*dq2.q(0) - dq1.q(1)*dq2.q(1) - dq1.q(2)*dq2.q(2) - dq1.q(3)*dq2.q(3), dq1.q(0)*dq2.q(1) + dq1.q(1)*dq2.q(0) + dq1.q(2)*dq2.q(3) - dq1.q(3)*dq2.q(2), dq1.q(0)*dq2.q(2) - dq1.q(1)*dq2.q(3) + dq1.q(2)*dq2.q(0) + dq1.q(3)*dq2.q(1), dq1.q(0)*dq2.q(3) + dq1.q(1)*dq2.q(2) - dq1.q(2)*dq2.q(1) + dq1.q(3)*dq2.q(0), - q4 + dq1_d.q(0)*dq2_p.q(0) - dq1_d.q(1)*dq2_p.q(1) - dq1_d.q(2)*dq2_p.q(2) - dq1_d.q(3)*dq2_p.q(3), - q5 + dq1_d.q(0)*dq2_p.q(1) + dq1_d.q(1)*dq2_p.q(0) + dq1_d.q(2)*dq2_p.q(3) - dq1_d.q(3)*dq2_p.q(2), - q6 + dq1_d.q(0)*dq2_p.q(2) - dq1_d.q(1)*dq2_p.q(3) + dq1_d.q(2)*dq2_p.q(0) + dq1_d.q(3)*dq2_p.q(1), - q7 + dq1_d.q(0)*dq2_p.q(3) + dq1_d.q(1)*dq2_p.q(2) - dq1_d.q(2)*dq2_p.q(1) + dq1_d.q(3)*dq2_p.q(0) - ); + q4 + dq1_d.q(0)*dq2_p.q(0) - dq1_d.q(1)*dq2_p.q(1) - + dq1_d.q(2)*dq2_p.q(2) - dq1_d.q(3)*dq2_p.q(3), + q5 + dq1_d.q(0)*dq2_p.q(1) + dq1_d.q(1)*dq2_p.q(0) + + dq1_d.q(2)*dq2_p.q(3) - dq1_d.q(3)*dq2_p.q(2), + q6 + dq1_d.q(0)*dq2_p.q(2) - dq1_d.q(1)*dq2_p.q(3) + + dq1_d.q(2)*dq2_p.q(0) + dq1_d.q(3)*dq2_p.q(1), + q7 + dq1_d.q(0)*dq2_p.q(3) + dq1_d.q(1)*dq2_p.q(2) - + dq1_d.q(2)*dq2_p.q(1) + dq1_d.q(3)*dq2_p.q(0)).finished());; } // Operator (==) overload From 1e64687d19a996c855df3d4d9dd19b913a50ffcd Mon Sep 17 00:00:00 2001 From: Frederico Afonso Date: Mon, 12 May 2025 17:55:03 +0100 Subject: [PATCH 2/3] Refactored classes for compliance with the new default constructor DQ::DQ(). --- src/robot_control/DQ_KinematicController.cpp | 12 +++-- src/robot_modeling/DQ_Kinematics.cpp | 15 ++++--- src/robot_modeling/DQ_MobileBase.cpp | 10 ++++- src/robot_modeling/DQ_SerialManipulator.cpp | 16 +++++-- src/robot_modeling/DQ_SerialManipulatorDH.cpp | 40 ++++++++++------- .../DQ_SerialManipulatorDenso.cpp | 36 +++++++++++++-- .../DQ_SerialManipulatorMDH.cpp | 44 +++++++++++-------- src/robot_modeling/DQ_SerialWholeBody.cpp | 8 +++- src/robot_modeling/DQ_WholeBody.cpp | 10 ++++- src/robots/FrankaEmikaPandaRobot.cpp | 10 ++++- 10 files changed, 145 insertions(+), 56 deletions(-) diff --git a/src/robot_control/DQ_KinematicController.cpp b/src/robot_control/DQ_KinematicController.cpp index 4704aff..02d3df5 100644 --- a/src/robot_control/DQ_KinematicController.cpp +++ b/src/robot_control/DQ_KinematicController.cpp @@ -17,7 +17,13 @@ This file is part of DQ Robotics. along with DQ Robotics. If not, see . Contributors: -- Murilo M. Marinho (murilomarinho@ieee.org) +1. Murilo M. Marinho (murilomarinho@ieee.org) + - Responsible for the original implementation. + +2. Frederico Fernandes Afonso Silva (frederico.silva@ieee.org) + - Refactored for compliance with the new default constructor DQ::DQ(). + [ffasilva committed on MM DD, 2025](COMMIT_NUMBER) + (LINK). */ #include @@ -54,8 +60,8 @@ DQ_KinematicController::DQ_KinematicController(const std::shared_ptr. Contributors: -- Murilo M. Marinho (murilomarinho@ieee.org) +1. Murilo M. Marinho (murilomarinho@ieee.org) + - Responsible for the original implementation. + +2. Frederico Fernandes Afonso Silva (frederico.silva@ieee.org) + - Refactored for compliance with the new default constructor DQ::DQ(). + [ffasilva committed on MM DD, 2025](COMMIT_NUMBER) + (LINK). */ #include @@ -29,11 +35,10 @@ This file is part of DQ Robotics. namespace DQ_robotics { -DQ_Kinematics::DQ_Kinematics(): - reference_frame_(1), - base_frame_(1) +DQ_Kinematics::DQ_Kinematics() { - + reference_frame_ = DQ((Matrix() << 1,0,0,0,0,0,0,0).finished()); + base_frame_ = DQ((Matrix() << 1,0,0,0,0,0,0,0).finished()); } /** diff --git a/src/robot_modeling/DQ_MobileBase.cpp b/src/robot_modeling/DQ_MobileBase.cpp index be31e22..3e2beff 100644 --- a/src/robot_modeling/DQ_MobileBase.cpp +++ b/src/robot_modeling/DQ_MobileBase.cpp @@ -17,7 +17,13 @@ This file is part of DQ Robotics. along with DQ Robotics. If not, see . Contributors: -- Murilo M. Marinho (murilomarinho@ieee.org) +1. Murilo M. Marinho (murilomarinho@ieee.org) + - Responsible for the original implementation. + +2. Frederico Fernandes Afonso Silva (frederico.silva@ieee.org) + - Refactored for compliance with the new default constructor DQ::DQ(). + [ffasilva committed on MM DD, 2025](COMMIT_NUMBER) + (LINK). */ #include @@ -27,7 +33,7 @@ namespace DQ_robotics DQ_MobileBase::DQ_MobileBase() { - frame_displacement_ = DQ(1); + frame_displacement_ = DQ((Matrix() << 1,0,0,0,0,0,0,0).finished()); } DQ DQ_MobileBase::frame_displacement() diff --git a/src/robot_modeling/DQ_SerialManipulator.cpp b/src/robot_modeling/DQ_SerialManipulator.cpp index 1d923af..2e1850d 100644 --- a/src/robot_modeling/DQ_SerialManipulator.cpp +++ b/src/robot_modeling/DQ_SerialManipulator.cpp @@ -17,9 +17,17 @@ This file is part of DQ Robotics. along with DQ Robotics. If not, see . Contributors: -- Murilo M. Marinho (murilomarinho@ieee.org) -- Mateus Rodrigues Martins (martinsrmateus@gmail.com) -- Juan Jose Quiroz Omana (juanjqo@g.ecc.u-tokyo.ac.jp) +1. Murilo M. Marinho (murilomarinho@ieee.org) + - Responsible for the original implementation. + +2. Mateus Rodrigues Martins (martinsrmateus@gmail.com) + +3. Juan Jose Quiroz Omana (juanjqo@g.ecc.u-tokyo.ac.jp) + +4. Frederico Fernandes Afonso Silva (frederico.silva@ieee.org) + - Refactored for compliance with the new default constructor DQ::DQ(). + [ffasilva committed on MM DD, 2025](COMMIT_NUMBER) + (LINK). */ #include @@ -33,7 +41,7 @@ namespace DQ_robotics DQ_SerialManipulator::DQ_SerialManipulator(const int &dim_configuration_space): DQ_Kinematics() { - curr_effector_ = DQ(1); + curr_effector_ = DQ((Matrix() << 1,0,0,0,0,0,0,0).finished()); lower_q_limit_.resize(dim_configuration_space); upper_q_limit_.resize(dim_configuration_space); lower_q_dot_limit_.resize(dim_configuration_space); diff --git a/src/robot_modeling/DQ_SerialManipulatorDH.cpp b/src/robot_modeling/DQ_SerialManipulatorDH.cpp index c40f0d9..66df2d3 100644 --- a/src/robot_modeling/DQ_SerialManipulatorDH.cpp +++ b/src/robot_modeling/DQ_SerialManipulatorDH.cpp @@ -17,8 +17,15 @@ This file is part of DQ Robotics. along with DQ Robotics. If not, see . Contributors: -- Murilo M. Marinho (murilomarinho@ieee.org) -- Juan Jose Quiroz Omana (juanjqo@g.ecc.u-tokyo.ac.jp) +1. Murilo M. Marinho (murilomarinho@ieee.org) + - Responsible for the original implementation. + +2. Juan Jose Quiroz Omana (juanjqo@g.ecc.u-tokyo.ac.jp) + +3. Frederico Fernandes Afonso Silva (frederico.silva@ieee.org) + - Refactored for compliance with the new default constructor DQ::DQ(). + [ffasilva committed on MM DD, 2025](COMMIT_NUMBER) + (LINK). */ #include @@ -87,16 +94,19 @@ DQ DQ_SerialManipulatorDH::_dh2dq(const double &q, const int &ith) const const double cosine_of_half_alpha = cos(half_alpha); // Return the optimized standard dh2dq calculation - return DQ( - cosine_of_half_alpha*cosine_of_half_theta, - sine_of_half_alpha*cosine_of_half_theta, - sine_of_half_alpha*sine_of_half_theta, - cosine_of_half_alpha*sine_of_half_theta, - -(a*sine_of_half_alpha*cosine_of_half_theta) /2.0 - (d*cosine_of_half_alpha*sine_of_half_theta)/2.0, - (a*cosine_of_half_alpha*cosine_of_half_theta)/2.0 - (d*sine_of_half_alpha*sine_of_half_theta )/2.0, - (a*cosine_of_half_alpha*sine_of_half_theta) /2.0 + (d*sine_of_half_alpha*cosine_of_half_theta)/2.0, - (d*cosine_of_half_alpha*cosine_of_half_theta)/2.0 - (a*sine_of_half_alpha*sine_of_half_theta )/2.0 - ); + return DQ((Matrix() << + cosine_of_half_alpha*cosine_of_half_theta, + sine_of_half_alpha*cosine_of_half_theta, + sine_of_half_alpha*sine_of_half_theta, + cosine_of_half_alpha*sine_of_half_theta, + -(a*sine_of_half_alpha*cosine_of_half_theta) /2.0 - + (d*cosine_of_half_alpha*sine_of_half_theta)/2.0, + (a*cosine_of_half_alpha*cosine_of_half_theta)/2.0 - + (d*sine_of_half_alpha*sine_of_half_theta )/2.0, + (a*cosine_of_half_alpha*sine_of_half_theta) /2.0 + + (d*sine_of_half_alpha*cosine_of_half_theta)/2.0, + (d*cosine_of_half_alpha*cosine_of_half_theta)/2.0 - + (a*sine_of_half_alpha*sine_of_half_theta )/2.0).finished()); } @@ -204,7 +214,7 @@ DQ DQ_SerialManipulatorDH::raw_fkm(const VectorXd& q_vec, const int& to_ith_lin _check_q_vec(q_vec); _check_to_ith_link(to_ith_link); - DQ q(1); + DQ q = DQ((Matrix() << 1,0,0,0,0,0,0,0).finished()); int j = 0; for (int i = 0; i < (to_ith_link+1); i++) { q = q * _dh2dq(q_vec(i-j), i); @@ -232,7 +242,7 @@ MatrixXd DQ_SerialManipulatorDH::raw_pose_jacobian(const VectorXd &q_vec, const MatrixXd J = MatrixXd::Zero(8,to_ith_link+1); DQ x_effector = raw_fkm(q_vec,to_ith_link); - DQ x(1); + DQ x = DQ((Matrix() << 1,0,0,0,0,0,0,0).finished()); for(int i=0;i() << 1,0,0,0,0,0,0,0).finished()); MatrixXd J_dot = MatrixXd::Zero(8,n); int jth=0; diff --git a/src/robot_modeling/DQ_SerialManipulatorDenso.cpp b/src/robot_modeling/DQ_SerialManipulatorDenso.cpp index 71eee93..6c682dc 100644 --- a/src/robot_modeling/DQ_SerialManipulatorDenso.cpp +++ b/src/robot_modeling/DQ_SerialManipulatorDenso.cpp @@ -1,3 +1,33 @@ +/** +(C) Copyright 2019-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) + - Responsible for the original implementation. + +2. Juan Jose Quiroz Omana (juanjqo@g.ecc.u-tokyo.ac.jp) + +3. Frederico Fernandes Afonso Silva (frederico.silva@ieee.org) + - Refactored for compliance with the new default constructor DQ::DQ(). + [ffasilva committed on MM DD, 2025](COMMIT_NUMBER) + (LINK). +*/ + #include namespace DQ_robotics { @@ -64,7 +94,7 @@ DQ DQ_SerialManipulatorDenso::raw_fkm(const VectorXd& q_vec, const int& to_ith_ _check_q_vec(q_vec); _check_to_ith_link(to_ith_link); - DQ q(1); + DQ q = DQ((Matrix() << 1,0,0,0,0,0,0,0).finished()); int j = 0; for (int i = 0; i < (to_ith_link+1); i++) { q = q * _denso2dh(q_vec(i-j), i); @@ -80,7 +110,7 @@ MatrixXd DQ_SerialManipulatorDenso::raw_pose_jacobian(const VectorXd &q_vec, con MatrixXd J = MatrixXd::Zero(8,to_ith_link+1); DQ x_effector = raw_fkm(q_vec,to_ith_link); - DQ x(1); + DQ x = DQ((Matrix() << 1,0,0,0,0,0,0,0).finished()); for(int i=0;i() << 1,0,0,0,0,0,0,0).finished()); MatrixXd J_dot = MatrixXd::Zero(8,n); for(int i=0;i. Contributors: -- Murilo M. Marinho (murilomarinho@ieee.org) -- Juan Jose Quiroz Omana - juanjqo@g.ecc.u-tokyo.ac.jp +1. Murilo M. Marinho (murilomarinho@ieee.org) + - Responsible for the original implementation. + +2. Juan Jose Quiroz Omana - juanjqo@g.ecc.u-tokyo.ac.jp + +3. Frederico Fernandes Afonso Silva (frederico.silva@ieee.org) + - Refactored for compliance with the new default constructor DQ::DQ(). + [ffasilva committed on MM DD, 2025](COMMIT_NUMBER) + (LINK). */ #include @@ -90,24 +97,23 @@ DQ DQ_SerialManipulatorMDH::_mdh2dq(const double &q, const int &ith) const const double cosine_of_half_alpha = cos(half_alpha); // Return the optimized standard mdh2dq calculation - return DQ( - cosine_of_half_alpha*cosine_of_half_theta, - sine_of_half_alpha*cosine_of_half_theta, - -sine_of_half_alpha*sine_of_half_theta, //MDH - cosine_of_half_alpha*sine_of_half_theta, + return DQ((Matrix() << + cosine_of_half_alpha*cosine_of_half_theta, + sine_of_half_alpha*cosine_of_half_theta, + -sine_of_half_alpha*sine_of_half_theta, //MDH + cosine_of_half_alpha*sine_of_half_theta, - -(a*sine_of_half_alpha*cosine_of_half_theta) /2.0 - - (d*cosine_of_half_alpha*sine_of_half_theta)/2.0, + -(a*sine_of_half_alpha*cosine_of_half_theta) /2.0 - + (d*cosine_of_half_alpha*sine_of_half_theta)/2.0, - (a*cosine_of_half_alpha*cosine_of_half_theta)/2.0 - - (d*sine_of_half_alpha*sine_of_half_theta )/2.0, + (a*cosine_of_half_alpha*cosine_of_half_theta)/2.0 - + (d*sine_of_half_alpha*sine_of_half_theta )/2.0, - -(a*cosine_of_half_alpha*sine_of_half_theta) /2.0 - - (d*sine_of_half_alpha*cosine_of_half_theta)/2.0, //MDH + -(a*cosine_of_half_alpha*sine_of_half_theta) /2.0 - + (d*sine_of_half_alpha*cosine_of_half_theta)/2.0, //MDH - (d*cosine_of_half_alpha*cosine_of_half_theta)/2.0 - - (a*sine_of_half_alpha*sine_of_half_theta )/2.0 - ); + (d*cosine_of_half_alpha*cosine_of_half_theta)/2.0 - + (a*sine_of_half_alpha*sine_of_half_theta )/2.0).finished()); } /** @@ -218,7 +224,7 @@ MatrixXd DQ_SerialManipulatorMDH::raw_pose_jacobian_derivative(const VectorXd &q MatrixXd J = raw_pose_jacobian(q,to_ith_link); VectorXd vec_x_effector_dot = J*q_dot.head(n); //(to_ith_link); - DQ x = DQ(1); + DQ x = DQ((Matrix() << 1,0,0,0,0,0,0,0).finished()); MatrixXd J_dot = MatrixXd::Zero(8,n); int jth=0; @@ -260,7 +266,7 @@ DQ DQ_SerialManipulatorMDH::raw_fkm(const VectorXd& q_vec, const int& to_ith_li _check_q_vec(q_vec); _check_to_ith_link(to_ith_link); - DQ q(1); + DQ q = DQ((Matrix() << 1,0,0,0,0,0,0,0).finished()); int j = 0; for (int i = 0; i < (to_ith_link+1); i++) { q = q * _mdh2dq(q_vec(i-j), i); @@ -288,7 +294,7 @@ MatrixXd DQ_SerialManipulatorMDH::raw_pose_jacobian(const VectorXd &q_vec, const MatrixXd J = MatrixXd::Zero(8,to_ith_link+1); DQ x_effector = raw_fkm(q_vec,to_ith_link); - DQ x(1); + DQ x = DQ((Matrix() << 1,0,0,0,0,0,0,0).finished()); for(int i=0;i @@ -161,7 +167,7 @@ DQ DQ_SerialWholeBody::raw_fkm_by_chain(const VectorXd &q, const int &to_ith_cha _check_q_vec(q); _check_to_ith_chain(to_ith_chain); - DQ pose(1); + DQ pose = DQ((Matrix() << 1,0,0,0,0,0,0,0).finished()); int q_counter = 0; int current_robot_dim; diff --git a/src/robot_modeling/DQ_WholeBody.cpp b/src/robot_modeling/DQ_WholeBody.cpp index 09a1ed7..6144abe 100644 --- a/src/robot_modeling/DQ_WholeBody.cpp +++ b/src/robot_modeling/DQ_WholeBody.cpp @@ -17,7 +17,13 @@ This file is part of DQ Robotics. along with DQ Robotics. If not, see . Contributors: -- Murilo M. Marinho (murilomarinho@ieee.org) +1. Murilo M. Marinho (murilomarinho@ieee.org) + - Responsible for the original implementation. + +2. Frederico Fernandes Afonso Silva (frederico.silva@ieee.org) + - Refactored for compliance with the new default constructor DQ::DQ(). + [ffasilva committed on MM DD, 2025](COMMIT_NUMBER) + (LINK). */ #include @@ -99,7 +105,7 @@ DQ DQ_WholeBody::raw_fkm(const VectorXd &q, const int &to_ith_chain) const { _check_to_ith_chain(to_ith_chain); - DQ pose(1); + DQ pose = DQ((Matrix() << 1,0,0,0,0,0,0,0).finished()); int q_counter = 0; for(int i=0;i. Contributors: -- Juan Jose Quiroz Omana (juanjqo@g.ecc.u-tokyo.ac.jp) +1. Juan Jose Quiroz Omana (juanjqo@g.ecc.u-tokyo.ac.jp) + - Responsible for the original implementation. + +2. Frederico Fernandes Afonso Silva (frederico.silva@ieee.org) + - Refactored for compliance with the new default constructor DQ::DQ(). + [ffasilva committed on MM DD, 2025](COMMIT_NUMBER) + (LINK). */ #include @@ -60,7 +66,7 @@ MatrixXd _get_mdh_matrix() */ DQ _get_offset_base() { - return 1 + E_ * 0.5 * DQ(0, 0.0413, 0, 0); + return 1 + E_ * 0.5 * i_*0.0413; } /** From 718079c34de702382341f34a692ca27290a1a101 Mon Sep 17 00:00:00 2001 From: Frederico Afonso Date: Mon, 19 May 2025 11:17:02 +0100 Subject: [PATCH 3/3] [All] Add PR date and link --- include/dqrobotics/DQ.h | 4 ++-- src/DQ.cpp | 4 ++-- src/robot_control/DQ_KinematicController.cpp | 4 ++-- src/robot_modeling/DQ_Kinematics.cpp | 4 ++-- src/robot_modeling/DQ_MobileBase.cpp | 4 ++-- src/robot_modeling/DQ_SerialManipulator.cpp | 4 ++-- src/robot_modeling/DQ_SerialManipulatorDH.cpp | 4 ++-- src/robot_modeling/DQ_SerialManipulatorDenso.cpp | 8 ++++---- src/robot_modeling/DQ_SerialManipulatorMDH.cpp | 4 ++-- src/robot_modeling/DQ_SerialWholeBody.cpp | 8 ++++---- src/robot_modeling/DQ_WholeBody.cpp | 8 ++++---- src/robots/FrankaEmikaPandaRobot.cpp | 8 ++++---- 12 files changed, 32 insertions(+), 32 deletions(-) diff --git a/include/dqrobotics/DQ.h b/include/dqrobotics/DQ.h index 7cf950b..48dc2e8 100644 --- a/include/dqrobotics/DQ.h +++ b/include/dqrobotics/DQ.h @@ -44,8 +44,8 @@ This file is part of DQ Robotics. - Deprecated the obsolete constructor with default double parameters, added the new default constructor that takes no arguments, and refactored for compliance. - [ffasilva committed on MM DD, 2025](COMMIT_NUMBER) - (LINK). + [ffasilva committed on May 19, 2025](PR #71) + (https://github.com/dqrobotics/cpp/pull/71). */ #pragma once diff --git a/src/DQ.cpp b/src/DQ.cpp index 40d485d..851e08c 100644 --- a/src/DQ.cpp +++ b/src/DQ.cpp @@ -44,8 +44,8 @@ This file is part of DQ Robotics. - Deprecated the obsolete constructor with default double parameters, added the new default constructor that takes no arguments, and refactored for compliance. - [ffasilva committed on MM DD, 2025](COMMIT_NUMBER) - (LINK). + [ffasilva committed on May 19, 2025](PR #71) + (https://github.com/dqrobotics/cpp/pull/71). */ #include diff --git a/src/robot_control/DQ_KinematicController.cpp b/src/robot_control/DQ_KinematicController.cpp index 02d3df5..4bcf114 100644 --- a/src/robot_control/DQ_KinematicController.cpp +++ b/src/robot_control/DQ_KinematicController.cpp @@ -22,8 +22,8 @@ This file is part of DQ Robotics. 2. Frederico Fernandes Afonso Silva (frederico.silva@ieee.org) - Refactored for compliance with the new default constructor DQ::DQ(). - [ffasilva committed on MM DD, 2025](COMMIT_NUMBER) - (LINK). + [ffasilva committed on May 19, 2025](PR #71) + (https://github.com/dqrobotics/cpp/pull/71). */ #include diff --git a/src/robot_modeling/DQ_Kinematics.cpp b/src/robot_modeling/DQ_Kinematics.cpp index b6f6d8d..81d13c9 100644 --- a/src/robot_modeling/DQ_Kinematics.cpp +++ b/src/robot_modeling/DQ_Kinematics.cpp @@ -22,8 +22,8 @@ This file is part of DQ Robotics. 2. Frederico Fernandes Afonso Silva (frederico.silva@ieee.org) - Refactored for compliance with the new default constructor DQ::DQ(). - [ffasilva committed on MM DD, 2025](COMMIT_NUMBER) - (LINK). + [ffasilva committed on May 19, 2025](PR #71) + (https://github.com/dqrobotics/cpp/pull/71). */ #include diff --git a/src/robot_modeling/DQ_MobileBase.cpp b/src/robot_modeling/DQ_MobileBase.cpp index 3e2beff..d798c02 100644 --- a/src/robot_modeling/DQ_MobileBase.cpp +++ b/src/robot_modeling/DQ_MobileBase.cpp @@ -22,8 +22,8 @@ This file is part of DQ Robotics. 2. Frederico Fernandes Afonso Silva (frederico.silva@ieee.org) - Refactored for compliance with the new default constructor DQ::DQ(). - [ffasilva committed on MM DD, 2025](COMMIT_NUMBER) - (LINK). + [ffasilva committed on May 19, 2025](PR #71) + (https://github.com/dqrobotics/cpp/pull/71). */ #include diff --git a/src/robot_modeling/DQ_SerialManipulator.cpp b/src/robot_modeling/DQ_SerialManipulator.cpp index fa435ec..5e57087 100644 --- a/src/robot_modeling/DQ_SerialManipulator.cpp +++ b/src/robot_modeling/DQ_SerialManipulator.cpp @@ -24,8 +24,8 @@ This file is part of DQ Robotics. 3. Frederico Fernandes Afonso Silva (frederico.silva@ieee.org) - Refactored for compliance with the new default constructor DQ::DQ(). - [ffasilva committed on MM DD, 2025](COMMIT_NUMBER) - (LINK). + [ffasilva committed on May 19, 2025](PR #71) + (https://github.com/dqrobotics/cpp/pull/71). */ #include diff --git a/src/robot_modeling/DQ_SerialManipulatorDH.cpp b/src/robot_modeling/DQ_SerialManipulatorDH.cpp index bc752ad..a5bbfd4 100644 --- a/src/robot_modeling/DQ_SerialManipulatorDH.cpp +++ b/src/robot_modeling/DQ_SerialManipulatorDH.cpp @@ -25,8 +25,8 @@ This file is part of DQ Robotics. 3. Frederico Fernandes Afonso Silva (frederico.silva@ieee.org) - Refactored for compliance with the new default constructor DQ::DQ(). - [ffasilva committed on MM DD, 2025](COMMIT_NUMBER) - (LINK). + [ffasilva committed on May 19, 2025](PR #71) + (https://github.com/dqrobotics/cpp/pull/71). */ #include diff --git a/src/robot_modeling/DQ_SerialManipulatorDenso.cpp b/src/robot_modeling/DQ_SerialManipulatorDenso.cpp index 6c682dc..a1b39ba 100644 --- a/src/robot_modeling/DQ_SerialManipulatorDenso.cpp +++ b/src/robot_modeling/DQ_SerialManipulatorDenso.cpp @@ -18,14 +18,14 @@ This file is part of DQ Robotics. Contributors: 1. Murilo M. Marinho (murilomarinho@ieee.org) - - Responsible for the original implementation. + - Responsible for the original implementation. 2. Juan Jose Quiroz Omana (juanjqo@g.ecc.u-tokyo.ac.jp) 3. Frederico Fernandes Afonso Silva (frederico.silva@ieee.org) - - Refactored for compliance with the new default constructor DQ::DQ(). - [ffasilva committed on MM DD, 2025](COMMIT_NUMBER) - (LINK). + - Refactored for compliance with the new default constructor DQ::DQ(). + [ffasilva committed on May 19, 2025](PR #71) + (https://github.com/dqrobotics/cpp/pull/71). */ #include diff --git a/src/robot_modeling/DQ_SerialManipulatorMDH.cpp b/src/robot_modeling/DQ_SerialManipulatorMDH.cpp index 13387c5..414cb70 100644 --- a/src/robot_modeling/DQ_SerialManipulatorMDH.cpp +++ b/src/robot_modeling/DQ_SerialManipulatorMDH.cpp @@ -25,8 +25,8 @@ This file is part of DQ Robotics. 3. Frederico Fernandes Afonso Silva (frederico.silva@ieee.org) - Refactored for compliance with the new default constructor DQ::DQ(). - [ffasilva committed on MM DD, 2025](COMMIT_NUMBER) - (LINK). + [ffasilva committed on May 19, 2025](PR #71) + (https://github.com/dqrobotics/cpp/pull/71). */ #include diff --git a/src/robot_modeling/DQ_SerialWholeBody.cpp b/src/robot_modeling/DQ_SerialWholeBody.cpp index 4546b6c..6d64a46 100644 --- a/src/robot_modeling/DQ_SerialWholeBody.cpp +++ b/src/robot_modeling/DQ_SerialWholeBody.cpp @@ -18,15 +18,15 @@ This file is part of DQ Robotics. Contributors: 1. Murilo M. Marinho (murilomarinho@ieee.org) - - Responsible for the original implementation. + - Responsible for the original implementation. 2. Juan Jose Quiroz Omana (juanjqogm@gmail.com) - Fixed bug 61 (https://github.com/dqrobotics/cpp/issues/61) in pose_jacobian method. 3. Frederico Fernandes Afonso Silva (frederico.silva@ieee.org) - - Refactored for compliance with the new default constructor DQ::DQ(). - [ffasilva committed on MM DD, 2025](COMMIT_NUMBER) - (LINK). + - Refactored for compliance with the new default constructor DQ::DQ(). + [ffasilva committed on May 19, 2025](PR #71) + (https://github.com/dqrobotics/cpp/pull/71). */ diff --git a/src/robot_modeling/DQ_WholeBody.cpp b/src/robot_modeling/DQ_WholeBody.cpp index 6144abe..0255533 100644 --- a/src/robot_modeling/DQ_WholeBody.cpp +++ b/src/robot_modeling/DQ_WholeBody.cpp @@ -18,12 +18,12 @@ This file is part of DQ Robotics. Contributors: 1. Murilo M. Marinho (murilomarinho@ieee.org) - - Responsible for the original implementation. + - Responsible for the original implementation. 2. Frederico Fernandes Afonso Silva (frederico.silva@ieee.org) - - Refactored for compliance with the new default constructor DQ::DQ(). - [ffasilva committed on MM DD, 2025](COMMIT_NUMBER) - (LINK). + - Refactored for compliance with the new default constructor DQ::DQ(). + [ffasilva committed on May 19, 2025](PR #71) + (https://github.com/dqrobotics/cpp/pull/71). */ #include diff --git a/src/robots/FrankaEmikaPandaRobot.cpp b/src/robots/FrankaEmikaPandaRobot.cpp index 46e082b..73e7c96 100644 --- a/src/robots/FrankaEmikaPandaRobot.cpp +++ b/src/robots/FrankaEmikaPandaRobot.cpp @@ -13,12 +13,12 @@ This file is part of DQ Robotics. along with DQ Robotics. If not, see . Contributors: 1. Juan Jose Quiroz Omana (juanjqo@g.ecc.u-tokyo.ac.jp) - - Responsible for the original implementation. + - Responsible for the original implementation. 2. Frederico Fernandes Afonso Silva (frederico.silva@ieee.org) - - Refactored for compliance with the new default constructor DQ::DQ(). - [ffasilva committed on MM DD, 2025](COMMIT_NUMBER) - (LINK). + - Refactored for compliance with the new default constructor DQ::DQ(). + [ffasilva committed on MM DD, 2025](PR #71) + (https://github.com/dqrobotics/cpp/pull/71). */ #include