Skip to content

This PR refactors DQ's default constructor #71

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Draft
wants to merge 5 commits into
base: master
Choose a base branch
from
Draft

This PR refactors DQ's default constructor #71

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
5 commits
Select commit Hold shift + click to select a range
27acf5a
[DQ] Deprecated the obsolete constructor with default double paramete…
ffasilva May 12, 2025
b5cd078
Merge branch 'dqrobotics:master' into default-constructor-refactor
ffasilva May 12, 2025
1e64687
Refactored classes for compliance with the new default constructor DQ…
ffasilva May 12, 2025
e0ff1f3
Merge with dqrobotics/cpp/master
ffasilva May 19, 2025
718079c
[All] Add PR date and link
ffasilva May 19, 2025
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
21 changes: 16 additions & 5 deletions include/dqrobotics/DQ.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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 May 19, 2025](PR #71)
(https://github.com/dqrobotics/cpp/pull/71).
*/
#pragma once

Expand Down Expand Up @@ -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,
Expand Down Expand Up @@ -287,10 +298,10 @@ std::ostream& operator<<(std::ostream &os, const DQ& dq);
Matrix<double,8,8> C8();
Matrix<double,4,4> 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<double,8,1>() << 0,1,0,0,0,0,0,0).finished());
const DQ j_((Matrix<double,8,1>() << 0,0,1,0,0,0,0,0).finished());
const DQ k_((Matrix<double,8,1>() << 0,0,0,1,0,0,0,0).finished());
const DQ E_((Matrix<double,8,1>() << 0,0,0,0,1,0,0,0).finished());

/*************************************************************************
************** DUAL QUATERNIONS AND SCALAR OPERATOR TEMPLATES ***********
Expand Down
84 changes: 65 additions & 19 deletions src/DQ.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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 May 19, 2025](PR #71)
(https://github.com/dqrobotics/cpp/pull/71).
*/

#include <dqrobotics/DQ.h>
Expand All @@ -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<double,8,1>() << 0,1,0,0,0,0,0,0).finished());
const DQ DQ::j((Matrix<double,8,1>() << 0,0,1,0,0,0,0,0).finished());
const DQ DQ::k((Matrix<double,8,1>() << 0,0,0,1,0,0,0,0).finished());
const DQ DQ::E((Matrix<double,8,1>() << 0,0,0,0,1,0,0,0).finished());

/****************************************************************
**************NAMESPACE ONLY FUNCTIONS***************************
Expand Down Expand Up @@ -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())
Expand Down Expand Up @@ -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<double,8,1>() << q(0),q(1),q(2),q(3),0,0,0,0).finished());
}


Expand All @@ -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<double,8,1>() << q(4),q(5),q(6),q(7),0,0,0,0).finished());
}


Expand All @@ -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<double,8,1>() << q(0),0,0,0,q(4),0,0,0).finished());
}

/**
Expand All @@ -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<double,8,1>() << 0,
q(1),
q(2),
q(3),
0,
q(5),
q(6),
q(7)).finished());
}

/**
Expand All @@ -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<double,8,1>() << q(0),
-q(1),
-q(2),
-q(3),
q(4),
-q(5),
-q(6),
-q(7)).finished());
}

/**
Expand Down Expand Up @@ -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<double,8,1>() << (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.
*
Expand Down Expand Up @@ -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<double,8,1>() << 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;

Expand Down Expand Up @@ -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<double,8,1>() << 1,0,0,0,0,0,0,0).finished());

exp = ( prim + E_*this->D()*prim );

Expand Down Expand Up @@ -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<double,8,1>() <<
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
Expand Down
12 changes: 9 additions & 3 deletions src/robot_control/DQ_KinematicController.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -17,7 +17,13 @@ This file is part of DQ Robotics.
along with DQ Robotics. If not, see <http://www.gnu.org/licenses/>.

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 May 19, 2025](PR #71)
(https://github.com/dqrobotics/cpp/pull/71).
*/

#include <dqrobotics/robot_control/DQ_KinematicController.h>
Expand Down Expand Up @@ -54,8 +60,8 @@ DQ_KinematicController::DQ_KinematicController(const std::shared_ptr<DQ_Kinemati
DQ_KinematicController::DQ_KinematicController():
robot_(nullptr),
control_objective_(ControlObjective::None),
attached_primitive_(0.0),
target_primitive_(0.0),
attached_primitive_(),
target_primitive_(),
gain_(0.0),
damping_(0),//Todo: change this inialization to use empty vector
system_reached_stable_region_(false),//Todo: change this inialization to use empty vector
Expand Down
15 changes: 10 additions & 5 deletions src/robot_modeling/DQ_Kinematics.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -17,7 +17,13 @@ This file is part of DQ Robotics.
along with DQ Robotics. If not, see <http://www.gnu.org/licenses/>.

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 May 19, 2025](PR #71)
(https://github.com/dqrobotics/cpp/pull/71).
*/

#include<dqrobotics/robot_modeling/DQ_Kinematics.h>
Expand All @@ -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<double,8,1>() << 1,0,0,0,0,0,0,0).finished());
base_frame_ = DQ((Matrix<double,8,1>() << 1,0,0,0,0,0,0,0).finished());
}

/**
Expand Down
10 changes: 8 additions & 2 deletions src/robot_modeling/DQ_MobileBase.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -17,7 +17,13 @@ This file is part of DQ Robotics.
along with DQ Robotics. If not, see <http://www.gnu.org/licenses/>.

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 May 19, 2025](PR #71)
(https://github.com/dqrobotics/cpp/pull/71).
*/

#include<dqrobotics/robot_modeling/DQ_MobileBase.h>
Expand All @@ -27,7 +33,7 @@ namespace DQ_robotics

DQ_MobileBase::DQ_MobileBase()
{
frame_displacement_ = DQ(1);
frame_displacement_ = DQ((Matrix<double,8,1>() << 1,0,0,0,0,0,0,0).finished());
}

DQ DQ_MobileBase::frame_displacement()
Expand Down
11 changes: 9 additions & 2 deletions src/robot_modeling/DQ_SerialManipulator.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -17,8 +17,15 @@ This file is part of DQ Robotics.
along with DQ Robotics. If not, see <http://www.gnu.org/licenses/>.

Contributors:
1. Murilo M. Marinho (murilomarinho@ieee.org)
1. Murilo M. Marinho (murilomarinho@ieee.org)
- Responsible for the original implementation.

2. Mateus Rodrigues Martins (martinsrmateus@gmail.com)

3. Frederico Fernandes Afonso Silva (frederico.silva@ieee.org)
- 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 <dqrobotics/robot_modeling/DQ_SerialManipulator.h>
Expand All @@ -32,7 +39,7 @@ namespace DQ_robotics
DQ_SerialManipulator::DQ_SerialManipulator(const int &dim_configuration_space):
DQ_Kinematics()
{
curr_effector_ = DQ(1);
curr_effector_ = DQ((Matrix<double,8,1>() << 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);
Expand Down
36 changes: 22 additions & 14 deletions src/robot_modeling/DQ_SerialManipulatorDH.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -20,8 +20,13 @@ This file is part of DQ Robotics.
1. Murilo M. Marinho (murilomarinho@ieee.org)
- Responsible for the original implementation.

2. Juan Jose Quiroz Omana (juanjqogm@gmail.com)
2. Juan Jose Quiroz Omana (juanjqo@g.ecc.u-tokyo.ac.jp)
- Added methods to get and set the DH parameters.

3. Frederico Fernandes Afonso Silva (frederico.silva@ieee.org)
- 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 <dqrobotics/robot_modeling/DQ_SerialManipulatorDH.h>
Expand Down Expand Up @@ -90,16 +95,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<double,8,1>() <<
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());
}

/**
Expand Down Expand Up @@ -303,7 +311,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<double,8,1>() << 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);
Expand Down Expand Up @@ -331,7 +339,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<double,8,1>() << 1,0,0,0,0,0,0,0).finished());

for(int i=0;i<to_ith_link+1;i++)
{
Expand Down Expand Up @@ -364,7 +372,7 @@ MatrixXd DQ_SerialManipulatorDH::raw_pose_jacobian_derivative(const VectorXd &q,
DQ x_effector = raw_fkm(q,to_ith_link);
MatrixXd J = raw_pose_jacobian(q,to_ith_link);
VectorXd vec_x_effector_dot = J*q_dot.head(n);
DQ x = DQ(1);
DQ x = DQ((Matrix<double,8,1>() << 1,0,0,0,0,0,0,0).finished());
MatrixXd J_dot = MatrixXd::Zero(8,n);
int jth=0;

Expand Down
Loading
Loading