From 1b3087d0dd192daa5dc8b2e190b0a5e3f632a80a Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Juan=20Jos=C3=A9=20Quiroz=20Oma=C3=B1a?=
 <juanjqogm@gmail.com>
Date: Fri, 11 Nov 2022 19:10:09 +0900
Subject: [PATCH 01/27] [DQ_Kinematics.h, .cpp] Added the
 pose_jacobian_derivative method.

---
 include/dqrobotics/robot_modeling/DQ_Kinematics.h |  5 +++++
 src/robot_modeling/DQ_Kinematics.cpp              | 13 +++++++++++++
 2 files changed, 18 insertions(+)

diff --git a/include/dqrobotics/robot_modeling/DQ_Kinematics.h b/include/dqrobotics/robot_modeling/DQ_Kinematics.h
index 6ca4912..0e728bb 100644
--- a/include/dqrobotics/robot_modeling/DQ_Kinematics.h
+++ b/include/dqrobotics/robot_modeling/DQ_Kinematics.h
@@ -64,8 +64,13 @@ class DQ_Kinematics
     virtual DQ fkm                (const VectorXd& joint_configurations) const = 0;
     virtual DQ fkm                (const VectorXd& joint_configurations, const int& to_ith_link) const = 0;
     virtual MatrixXd pose_jacobian(const VectorXd& joint_configurations, const int& to_ith_link) const = 0;
+    virtual MatrixXd pose_jacobian_derivative(const VectorXd& joint_configurations,
+                                              const VectorXd& joint_velocity_configurations,
+                                              const int& to_ith_link) const = 0;
     //Virtual methods
     virtual MatrixXd pose_jacobian (const VectorXd& joint_configurations) const;
+    virtual MatrixXd pose_jacobian_derivative(const VectorXd& joint_configurations,
+                                              const VectorXd& joint_velocity_configurations) const;
     virtual int get_dim_configuration_space() const;
 
     //Static methods
diff --git a/src/robot_modeling/DQ_Kinematics.cpp b/src/robot_modeling/DQ_Kinematics.cpp
index 3d93d60..2f2323b 100644
--- a/src/robot_modeling/DQ_Kinematics.cpp
+++ b/src/robot_modeling/DQ_Kinematics.cpp
@@ -131,6 +131,19 @@ MatrixXd DQ_Kinematics::pose_jacobian(const VectorXd &joint_configurations) cons
     return pose_jacobian(joint_configurations, get_dim_configuration_space()-1);
 }
 
+/**
+ * @brief returns the Jacobian derivative 'J_dot' that satisfies
+          vec8(pose_dot_dot) = J_dot * q_dot + J*q_dot_dot, where pose = fkm(), 'pose_dot' is the time
+          derivative of the pose and joint_configurations is the configuration vector.
+ * @param joint_configurations The VectorXd representing the joint configurations.
+ * @param joint_velocity_configurations The VectorXd representing the joint velocity configurations.
+ * @return a MatrixXd representing the desired Jacobian derivative.
+ */
+MatrixXd DQ_Kinematics::pose_jacobian_derivative(const VectorXd &joint_configurations, const VectorXd &joint_velocity_configurations) const
+{
+    return pose_jacobian_derivative(joint_configurations, joint_velocity_configurations, get_dim_configuration_space()-1);
+}
+
 /**
  * @brief Returns the dimensions of the configuration space.
  * @return An int presenting the dimension of the configuration space.

From c8373abc57ab7ed26492f4c3744efc0605d57d57 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Juan=20Jos=C3=A9=20Quiroz=20Oma=C3=B1a?=
 <juanjqogm@gmail.com>
Date: Fri, 11 Nov 2022 19:25:43 +0900
Subject: [PATCH 02/27] [DQ_HolonomicBase.h, .cpp] Added the
 pose_jacobian_derivative method.

---
 .../robot_modeling/DQ_HolonomicBase.h         |  6 ++
 src/robot_modeling/DQ_HolonomicBase.cpp       | 77 +++++++++++++++++++
 2 files changed, 83 insertions(+)

diff --git a/include/dqrobotics/robot_modeling/DQ_HolonomicBase.h b/include/dqrobotics/robot_modeling/DQ_HolonomicBase.h
index 78b805e..b3304f0 100644
--- a/include/dqrobotics/robot_modeling/DQ_HolonomicBase.h
+++ b/include/dqrobotics/robot_modeling/DQ_HolonomicBase.h
@@ -40,9 +40,15 @@ class DQ_HolonomicBase: public DQ_MobileBase
     virtual DQ fkm(const VectorXd& q, const int& to_ith_link) const override;
     virtual MatrixXd pose_jacobian(const VectorXd& q, const int& to_link) const override;
     virtual MatrixXd pose_jacobian(const VectorXd& q) const override;
+    virtual MatrixXd pose_jacobian_derivative(const VectorXd& q,
+                                              const VectorXd& q_dot, const int& to_link) const override;
+    virtual MatrixXd pose_jacobian_derivative(const VectorXd& q,
+                                              const VectorXd& q_dot) const override;
 
     DQ raw_fkm(const VectorXd& q) const;
     MatrixXd raw_pose_jacobian(const VectorXd& q, const int& to_link=2) const;
+    MatrixXd raw_pose_jacobian_derivative(const VectorXd& q,
+                                          const VectorXd& q_dot, const int& to_link = 2) const;
 };
 
 }
diff --git a/src/robot_modeling/DQ_HolonomicBase.cpp b/src/robot_modeling/DQ_HolonomicBase.cpp
index 025edae..2e75146 100644
--- a/src/robot_modeling/DQ_HolonomicBase.cpp
+++ b/src/robot_modeling/DQ_HolonomicBase.cpp
@@ -107,4 +107,81 @@ MatrixXd DQ_HolonomicBase::pose_jacobian(const VectorXd &q) const
     return pose_jacobian(q,get_dim_configuration_space()-1);
 }
 
+/**
+ * @brief returns the Jacobian derivative (J_dot) that satisfies
+          vec8(pose_dot_dot) = J_dot * q_dot + J*q_dot_dot, where pose = fkm(), 'pose_dot' is the time
+          derivative of the pose and  joint_configurations is the configuration vector.
+          This method does not take into account the base displacement.
+ * @param joint_configurations The VectorXd representing the joint configurations.
+ * @param joint_velocity_configurations The VectorXd representing the joint velocity configurations.
+ * @param to_link The ith link which we want to compute the Jacobian derivative.
+ * @return a MatrixXd representing the desired Jacobian derivative.
+ */
+MatrixXd DQ_HolonomicBase::raw_pose_jacobian_derivative(const VectorXd &q, const VectorXd &q_dot, const int &to_link) const
+{
+    if(to_link >= 3 || to_link < 0)
+    {
+        throw std::runtime_error(std::string("Tried to access link index ") + std::to_string(to_link) + std::string(" which is unnavailable."));
+    }
+    const double& x   = q(0);
+    const double& y   = q(1);
+    const double& phi = q(2);
+
+    const double& x_dot   = q_dot(0);
+    const double& y_dot   = q_dot(1);
+    const double& phi_dot = q_dot(2);
+
+    const double c = cos(phi/2.0);
+    const double s = sin(phi/2.0);
+
+    const double j71 = -0.25*c*phi_dot;
+    const double j62 = -j71;
+    const double j13 = -j62;
+
+    const double j72 = -0.25*s*phi_dot;
+    const double j61 = j72;
+    const double j43 = j61;
+
+    const double j63 = 0.25 * (-x_dot*s - 0.5*x*c*phi_dot + y_dot*c - 0.5*y*s*phi_dot);
+    const double j73 = 0.25 * (-x_dot*c + 0.5*x*s*phi_dot - y_dot*s - 0.5*y*c*phi_dot);
+
+    MatrixXd J_dot(8,3);
+    J_dot <<    0.0, 0.0, j13,
+            0.0, 0.0, 0.0,
+            0.0, 0.0, 0.0,
+            0.0, 0.0, j43,
+            0.0, 0.0, 0.0,
+            j61, j62, j63,
+            j71, j72, j73,
+            0.0, 0.0, 0.0;
+    return J_dot.block(0,0,8,to_link+1);
+}
+
+/**
+ * @brief returns the Jacobian derivative (J_dot) that satisfies
+          vec8(pose_dot_dot) = J_dot * q_dot + J*q_dot_dot, where pose = fkm(), 'pose_dot' is the time
+          derivative of the pose and  joint_configurations is the configuration vector.
+ * @param joint_configurations The VectorXd representing the joint configurations.
+ * @param joint_velocity_configurations The VectorXd representing the joint velocity configurations.
+ * @param to_link The ith link which we want to compute the Jacobian derivative.
+ * @return a MatrixXd representing the desired Jacobian derivative.
+ */
+MatrixXd DQ_HolonomicBase::pose_jacobian_derivative(const VectorXd &q, const VectorXd &q_dot, const int &to_link) const
+{
+    return haminus8(frame_displacement_)*raw_pose_jacobian_derivative(q, q_dot, to_link);
+}
+
+/**
+ * @brief returns the Jacobian derivative (J_dot) that satisfies
+          vec8(pose_dot_dot) = J_dot * q_dot + J*q_dot_dot, where pose = fkm(), 'pose_dot' is the time
+          derivative of the pose and  joint_configurations is the configuration vector.
+ * @param joint_configurations The VectorXd representing the joint configurations.
+ * @param joint_velocity_configurations The VectorXd representing the joint velocity configurations.
+ * @return a MatrixXd representing the desired Jacobian derivative.
+ */
+MatrixXd DQ_HolonomicBase::pose_jacobian_derivative(const VectorXd &q, const VectorXd &q_dot) const
+{
+    return pose_jacobian_derivative(q, q_dot, get_dim_configuration_space()-1);
+}
+
 }

From d75a1f4b2935fef165c673b1b68016c6ac809232 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Juan=20Jos=C3=A9=20Quiroz=20Oma=C3=B1a?=
 <juanjqogm@gmail.com>
Date: Fri, 11 Nov 2022 19:51:51 +0900
Subject: [PATCH 03/27] [DQ_SerialWholeBody.h, .cpp] Added the
 pose_jacobian_derivative method. However, it is not implemented yet.

---
 .../robot_modeling/DQ_SerialWholeBody.h       | 11 ++++-
 src/robot_modeling/DQ_SerialWholeBody.cpp     | 40 ++++++++++++++++++-
 2 files changed, 49 insertions(+), 2 deletions(-)

diff --git a/include/dqrobotics/robot_modeling/DQ_SerialWholeBody.h b/include/dqrobotics/robot_modeling/DQ_SerialWholeBody.h
index 609d7e3..9721ce9 100644
--- a/include/dqrobotics/robot_modeling/DQ_SerialWholeBody.h
+++ b/include/dqrobotics/robot_modeling/DQ_SerialWholeBody.h
@@ -1,5 +1,5 @@
 /**
-(C) Copyright 2020 DQ Robotics Developers
+(C) Copyright 2020-2022 DQ Robotics Developers
 
 This file is part of DQ Robotics.
 
@@ -58,12 +58,21 @@ class DQ_SerialWholeBody : public DQ_Kinematics
     DQ_SerialManipulatorDH get_chain_as_serial_manipulator_dh(const int& to_ith_chain) const;
     DQ_HolonomicBase get_chain_as_holonomic_base(const int& to_ith_chain) const;
     MatrixXd raw_pose_jacobian_by_chain(const VectorXd& q, const int& to_ith_chain, const int& to_jth_link) const;
+    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
     DQ fkm(const VectorXd& q) const override;
     DQ fkm(const VectorXd&, const int& to_ith_link) const override;
     MatrixXd pose_jacobian(const VectorXd& q, const int& to_ith_link) const override;
     MatrixXd pose_jacobian(const VectorXd& q) const override;
+    MatrixXd pose_jacobian_derivative(const VectorXd& q,
+                                      const VectorXd& q_dot,
+                                      const int& to_ith_link) const override; //To be implemented.
+    MatrixXd pose_jacobian_derivative (const VectorXd& q,
+                                       const VectorXd& q_dot) const override; //To be implemented.
 };
 
 }
diff --git a/src/robot_modeling/DQ_SerialWholeBody.cpp b/src/robot_modeling/DQ_SerialWholeBody.cpp
index eec7378..44f8884 100644
--- a/src/robot_modeling/DQ_SerialWholeBody.cpp
+++ b/src/robot_modeling/DQ_SerialWholeBody.cpp
@@ -1,5 +1,5 @@
 /**
-(C) Copyright 2020 DQ Robotics Developers
+(C) Copyright 2020-2022 DQ Robotics Developers
 
 This file is part of DQ Robotics.
 
@@ -250,6 +250,44 @@ MatrixXd DQ_SerialWholeBody::pose_jacobian(const VectorXd &q) const
     return pose_jacobian(q, get_dim_configuration_space()-1);
 }
 
+//To be implemented.
+MatrixXd DQ_SerialWholeBody::raw_pose_jacobian_derivative_by_chain(const VectorXd &q, const VectorXd &q_dot, const int &to_ith_chain, const int &to_jth_link) const
+{
+    throw std::runtime_error(std::string("pose_jacobian_derivative_by_chain is not implemented yet."));
+    return MatrixXd::Zero(1,1);
+}
+
+/**
+ * @brief returns the Jacobian derivative 'J_dot' that satisfies
+          vec8(pose_dot_dot) = J_dot * q_dot + J*q_dot_dot, where pose = fkm(), 'pose_dot' is the time
+          derivative of the pose and joint_configurations is the configuration vector.
+ * @param q The VectorXd representing the joint configurations.
+ * @param q_dot The VectorXd representing the joint velocity configurations.
+ * @param to_ith_link The 'to_ith_link' link which we want to compute the Jacobian derivative.
+ * @return a MatrixXd representing the desired Jacobian derivative.
+ */
+MatrixXd DQ_SerialWholeBody::pose_jacobian_derivative(const VectorXd &q, const VectorXd &q_dot, const int &to_ith_link) const
+{
+    int to_ith_chain;
+    int to_jth_link;
+    std::tie(to_ith_chain,to_jth_link) = get_chain_and_link_from_index(to_ith_link);
+    return raw_pose_jacobian_derivative_by_chain(q, q_dot, to_ith_chain,to_ith_link);
+}
+
+
+/**
+ * @brief returns the Jacobian derivative 'J_dot' that satisfies
+          vec8(pose_dot_dot) = J_dot * q_dot + J*q_dot_dot, where pose = fkm(), 'pose_dot' is the time
+          derivative of the pose and joint_configurations is the configuration vector.
+ * @param q The VectorXd representing the joint configurations.
+ * @param q_dot The VectorXd representing the joint velocity configurations.
+ * @return a MatrixXd representing the desired Jacobian derivative.
+ */
+MatrixXd DQ_SerialWholeBody::pose_jacobian_derivative(const VectorXd &q, const VectorXd &q_dot) const
+{
+    return pose_jacobian_derivative(q,q_dot,  get_dim_configuration_space()-1);
+}
+
 void DQ_SerialWholeBody::set_effector(const DQ &effector)
 {
     try

From b31bfa744ab1685b37c3d92367dd5f9a39f21fe5 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Juan=20Jos=C3=A9=20Quiroz=20Oma=C3=B1a?=
 <juanjqogm@gmail.com>
Date: Fri, 11 Nov 2022 20:34:45 +0900
Subject: [PATCH 04/27] [DQ_DifferentialDriveRobot.h, .cpp] Added the
 pose_jacobian_derivative method.

---
 .../DQ_DifferentialDriveRobot.h               |  9 ++-
 .../DQ_DifferentialDriveRobot.cpp             | 56 ++++++++++++++++++-
 2 files changed, 62 insertions(+), 3 deletions(-)

diff --git a/include/dqrobotics/robot_modeling/DQ_DifferentialDriveRobot.h b/include/dqrobotics/robot_modeling/DQ_DifferentialDriveRobot.h
index 23beabe..33d4988 100644
--- a/include/dqrobotics/robot_modeling/DQ_DifferentialDriveRobot.h
+++ b/include/dqrobotics/robot_modeling/DQ_DifferentialDriveRobot.h
@@ -1,5 +1,5 @@
 /**
-(C) Copyright 2019 DQ Robotics Developers
+(C) Copyright 2019-2022 DQ Robotics Developers
 
 This file is part of DQ Robotics.
 
@@ -38,9 +38,14 @@ class DQ_DifferentialDriveRobot : public DQ_HolonomicBase
     DQ_DifferentialDriveRobot(const double& wheel_radius, const double& distance_between_wheels);
 
     MatrixXd constraint_jacobian(const double& phi) const;
-
+    MatrixXd constraint_jacobian_derivative(const double& phi, const double& phi_dot) const;
     MatrixXd pose_jacobian(const VectorXd& q, const int& to_link) const override;
     MatrixXd pose_jacobian(const VectorXd &q) const override;
+    MatrixXd pose_jacobian_derivative(const VectorXd& q,
+                                      const VectorXd& q_dot,
+                                      const int& to_link) const override;
+    MatrixXd pose_jacobian_derivative (const VectorXd& q,
+                                       const VectorXd& q_dot) const override;
 };
 
 }
diff --git a/src/robot_modeling/DQ_DifferentialDriveRobot.cpp b/src/robot_modeling/DQ_DifferentialDriveRobot.cpp
index fcfd175..07406c5 100644
--- a/src/robot_modeling/DQ_DifferentialDriveRobot.cpp
+++ b/src/robot_modeling/DQ_DifferentialDriveRobot.cpp
@@ -1,5 +1,5 @@
 /**
-(C) Copyright 2019 DQ Robotics Developers
+(C) Copyright 2019-2022 DQ Robotics Developers
 
 This file is part of DQ Robotics.
 
@@ -45,6 +45,25 @@ MatrixXd DQ_DifferentialDriveRobot::constraint_jacobian(const double &phi) const
     return J;
 }
 
+/**
+ * @brief returns the time derivative of the constraint Jacobian
+ * @param phi The orientation of the robot on the plane.
+ * @param phi_dot The time derivative of phi.
+ * @return a MatrixXd representing the desired Jacobian derivative
+ */
+MatrixXd DQ_DifferentialDriveRobot::constraint_jacobian_derivative(const double &phi, const double &phi_dot) const
+{
+    const double& r = wheel_radius_;
+    double c = cos(phi);
+    double s = sin(phi);
+
+    MatrixXd J_dot(3,2);
+    J_dot << -(r/2)*s, -(r/2)*s,
+            (r/2)*c, (r/2)*c,
+               0, 0;
+    return J_dot*phi_dot;
+}
+
 MatrixXd DQ_DifferentialDriveRobot::pose_jacobian(const VectorXd &q, const int &to_link) const
 {
     if(to_link!=0 && to_link!=1)
@@ -63,4 +82,39 @@ MatrixXd DQ_DifferentialDriveRobot::pose_jacobian(const VectorXd &q) const
     return pose_jacobian(q,1);
 }
 
+
+/**
+ * @brief returns the pose Jacobian derivative
+ * @param q The VectorXd representing the robot configuration.
+ * @param q_dot The VectorXd representing the robot configuration velocity.
+ * @param to_link The ith link which we want to compute the Jacobian derivative.
+ * @return a MatrixXd representing the desired Jacobian
+ */
+MatrixXd DQ_DifferentialDriveRobot::pose_jacobian_derivative(const VectorXd &q, const VectorXd &q_dot, const int &to_link) const
+{
+    if(to_link!=0 && to_link!=1)
+        throw std::runtime_error("DQ_DifferentialDriveRobot::pose_jacobian_derivative(q,q_dot, to_link) only accepts to_link in {0,1}.");
+
+    MatrixXd J_holonomic = DQ_HolonomicBase::pose_jacobian(q,2);
+    MatrixXd J_holonomic_dot = DQ_HolonomicBase::pose_jacobian_derivative(q,q_dot,2);
+    MatrixXd J_c = constraint_jacobian(q(2));
+    MatrixXd J_c_dot = constraint_jacobian_derivative(q(2), q_dot(2));
+    MatrixXd J_dot = J_holonomic_dot*J_c + J_holonomic*J_c_dot;
+    return J_dot.block(0,0,8,to_link+1);
+}
+
+/**
+ * @brief returns the pose Jacobian derivative
+ * @param q The VectorXd representing the robot configuration.
+ * @param q_dot The VectorXd representing the robot configuration velocity.
+ * @return a MatrixXd representing the desired Jacobian
+ */
+MatrixXd DQ_DifferentialDriveRobot::pose_jacobian_derivative(const VectorXd &q, const VectorXd &q_dot) const
+{
+    // The DQ_DifferentialDriveRobot works differently from most other subclasses of DQ_Kinematics
+    // The size of the configuration space is three but there is one constraint, so there are only
+    // to columns in the pose_jacobian
+    return pose_jacobian_derivative(q, q_dot, 1);
+}
+
 }

From 4b4136936a18e984ade50e4e0d791460796023a3 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Juan=20Jos=C3=A9=20Quiroz=20Oma=C3=B1a?=
 <juanjqogm@gmail.com>
Date: Fri, 11 Nov 2022 21:09:33 +0900
Subject: [PATCH 05/27] [DQ_SerialManipulator.h, .cpp] Added the
 pose_jacobian_derivative method.

---
 .../robot_modeling/DQ_SerialManipulator.h     |  6 ++
 src/robot_modeling/DQ_SerialManipulator.cpp   | 62 ++++++++++++++++++-
 2 files changed, 67 insertions(+), 1 deletion(-)

diff --git a/include/dqrobotics/robot_modeling/DQ_SerialManipulator.h b/include/dqrobotics/robot_modeling/DQ_SerialManipulator.h
index 6b19720..525c6e1 100644
--- a/include/dqrobotics/robot_modeling/DQ_SerialManipulator.h
+++ b/include/dqrobotics/robot_modeling/DQ_SerialManipulator.h
@@ -20,6 +20,7 @@ This file is part of DQ Robotics.
 Contributors:
 - Murilo M. Marinho        (murilo@nml.t.u-tokyo.ac.jp)
 - Mateus Rodrigues Martins (martinsrmateus@gmail.com)
+- Juan Jose Quiroz Omana   (juanjqo@g.ecc.u-tokyo.ac.jp)
 */
 
 #include <dqrobotics/robot_modeling/DQ_Kinematics.h>
@@ -49,10 +50,12 @@ class DQ_SerialManipulator: public DQ_Kinematics
 
     //Virtual
     virtual MatrixXd raw_pose_jacobian(const VectorXd& q_vec) const;
+    virtual MatrixXd raw_pose_jacobian_derivative(const VectorXd& q_vec, const VectorXd& q_vec_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_vec, const VectorXd& q_vec_dot, const int& to_ith_link) const = 0;
     virtual DQ raw_fkm(const VectorXd& q_vec, const int& to_ith_link) const = 0;
 
     //Overrides from DQ_Kinematics
@@ -63,6 +66,9 @@ class DQ_SerialManipulator: public 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_vec, const VectorXd& q_vec_dot, const int& to_ith_link) const override; //Override from DQ_Kinematics
+    virtual MatrixXd pose_jacobian_derivative(const VectorXd& q_vec, const VectorXd& q_vec_dot) const override; //Override from DQ_Kinematics
+
 };
 
 }
diff --git a/src/robot_modeling/DQ_SerialManipulator.cpp b/src/robot_modeling/DQ_SerialManipulator.cpp
index 44a79b8..3c1f388 100644
--- a/src/robot_modeling/DQ_SerialManipulator.cpp
+++ b/src/robot_modeling/DQ_SerialManipulator.cpp
@@ -1,5 +1,5 @@
 /**
-(C) Copyright 2011-2018 DQ Robotics Developers
+(C) Copyright 2011-2020 DQ Robotics Developers
 
 This file is part of DQ Robotics.
 
@@ -19,6 +19,7 @@ This file is part of DQ Robotics.
 Contributors:
 - Murilo M. Marinho (murilo@nml.t.u-tokyo.ac.jp)
 - Mateus Rodrigues Martins (martinsrmateus@gmail.com)
+- Juan Jose Quiroz Omana   (juanjqo@g.ecc.u-tokyo.ac.jp)
 */
 
 #include <dqrobotics/robot_modeling/DQ_SerialManipulator.h>
@@ -156,5 +157,64 @@ MatrixXd DQ_SerialManipulator::pose_jacobian(const VectorXd &q_vec) const
     return this->DQ_Kinematics::pose_jacobian(q_vec);
 }
 
+
+/**
+ * @brief This method returns the time derivative of the pose Jacobian.
+ *        The base displacement and the effector are not taken into account.
+ * @param q_vec. Vector of joint values.
+ * @param q_vec_dot. Vector of joint velocity values.
+                  will be calculated.
+ * @returns The first to_ith_link columns of the pose_jacobian_derivative.
+ *
+ */
+MatrixXd DQ_SerialManipulator::raw_pose_jacobian_derivative(const VectorXd &q_vec, const VectorXd &q_vec_dot) const
+{
+    _check_q_vec(q_vec);
+    _check_q_vec(q_vec_dot);
+    return raw_pose_jacobian_derivative(q_vec, q_vec_dot, get_dim_configuration_space()-1);
+}
+
+
+/**
+ * @brief This method returns the first to_ith_link columns of the time derivative of the pose Jacobian.
+ * @param q_vec. Vector of joint values.
+ * @param q_vec_dot. Vector of joint velocity values.
+ * @param to_ith_link. The index to a link. This defines until which link the pose_jacobian_derivative
+ *                     will be calculated.
+ * @returns The first to_ith_link columns of the pose_jacobian_derivative.
+ *
+ */
+MatrixXd  DQ_SerialManipulator::pose_jacobian_derivative(const VectorXd &q_vec, const VectorXd &q_vec_dot, const int &to_ith_link) const
+{
+    _check_q_vec(q_vec);
+    _check_q_vec(q_vec_dot);
+    _check_to_ith_link(to_ith_link);
+    MatrixXd J_dot = raw_pose_jacobian_derivative(q_vec, q_vec_dot, to_ith_link);
+
+    if(to_ith_link==this->get_dim_configuration_space()-1)
+    {
+        J_dot = hamiplus8(reference_frame_)*haminus8(curr_effector_)*J_dot;
+    }
+    else
+    {
+        J_dot = hamiplus8(reference_frame_)*J_dot;
+    }
+    return J_dot;
+}
+
+/**
+ * @brief This method returns time derivative of the pose Jacobian.
+ * @param q_vec. Vector of joint values.
+ * @param q_vec_dot. Vector of joint velocity values.
+ * @returns The desired pose Jacobian derivative.
+ *
+ */
+MatrixXd DQ_SerialManipulator::pose_jacobian_derivative(const VectorXd &q_vec, const VectorXd &q_vec_dot) const
+{
+    _check_q_vec(q_vec);
+    _check_q_vec(q_vec_dot);
+    return this->DQ_Kinematics::pose_jacobian_derivative(q_vec, q_vec_dot);
+}
+
 }//namespace DQ_robotics
 

From 4a7b47ef1a54bf83040ee3500976e37e1e4b0e51 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Juan=20Jos=C3=A9=20Quiroz=20Oma=C3=B1a?=
 <juanjqogm@gmail.com>
Date: Fri, 11 Nov 2022 21:09:52 +0900
Subject: [PATCH 06/27] [DQ_SerialManipulatorDH.h, .cpp] Added the
 pose_jacobian_derivative method.

---
 .../robot_modeling/DQ_SerialManipulatorDH.h   |   6 +-
 src/robot_modeling/DQ_SerialManipulatorDH.cpp | 109 ++++++++----------
 2 files changed, 50 insertions(+), 65 deletions(-)

diff --git a/include/dqrobotics/robot_modeling/DQ_SerialManipulatorDH.h b/include/dqrobotics/robot_modeling/DQ_SerialManipulatorDH.h
index c035e4a..c028b57 100644
--- a/include/dqrobotics/robot_modeling/DQ_SerialManipulatorDH.h
+++ b/include/dqrobotics/robot_modeling/DQ_SerialManipulatorDH.h
@@ -19,6 +19,7 @@ This file is part of DQ Robotics.
 
 Contributors:
 - Murilo M. Marinho (murilo@nml.t.u-tokyo.ac.jp)
+- Juan Jose Quiroz Omana   (juanjqo@g.ecc.u-tokyo.ac.jp)
 */
 
 
@@ -47,13 +48,12 @@ class DQ_SerialManipulatorDH: public DQ_SerialManipulator
     DQ_SerialManipulatorDH()=delete;
     DQ_SerialManipulatorDH(const MatrixXd& dh_matrix);
 
-    MatrixXd pose_jacobian_derivative(const VectorXd& q_vec, const VectorXd& q_vec_dot, const int& to_ith_link) const;
-    MatrixXd pose_jacobian_derivative(const VectorXd& q_vec, const VectorXd& q_vec_dot) const;
-
     using DQ_SerialManipulator::raw_pose_jacobian;
+    using DQ_SerialManipulator::raw_pose_jacobian_derivative;
     using DQ_SerialManipulator::raw_fkm;
 
     MatrixXd raw_pose_jacobian(const VectorXd& q_vec, const int& to_ith_link) const override;
+    MatrixXd raw_pose_jacobian_derivative(const VectorXd& q_vec, const VectorXd& q_vec_dot, const int& to_ith_link) const override;
     DQ raw_fkm(const VectorXd &q_vec, const int &to_ith_link) const override;
 };
 
diff --git a/src/robot_modeling/DQ_SerialManipulatorDH.cpp b/src/robot_modeling/DQ_SerialManipulatorDH.cpp
index 2bc9f16..88eeeff 100644
--- a/src/robot_modeling/DQ_SerialManipulatorDH.cpp
+++ b/src/robot_modeling/DQ_SerialManipulatorDH.cpp
@@ -18,6 +18,7 @@ This file is part of DQ Robotics.
 
 Contributors:
 - Murilo M. Marinho (murilo@nml.t.u-tokyo.ac.jp)
+- Juan Jose Quiroz Omana   (juanjqo@g.ecc.u-tokyo.ac.jp)
 */
 
 #include <dqrobotics/robot_modeling/DQ_SerialManipulatorDH.h>
@@ -189,68 +190,6 @@ VectorXd DQ_SerialManipulatorDH::get_types() const
     return dh_matrix_.row(4);
 }
 
-
-/**
- * @brief This method returns the first to_ith_link columns of the pose Jacobian time derivative
- * @param q_vec. Vector of joint values.
- * @param q_vec_dot. Vector of joint velocity values.
- * @param to_ith_link. The index to a link. This defines until which link the pose_jacobian_derivative
- *                     will be calculated.
- * @returns The first to_ith_link columns of the pose_jacobian_derivative.
- *
- */
-MatrixXd DQ_SerialManipulatorDH::pose_jacobian_derivative(const VectorXd &q_vec, const VectorXd &q_vec_dot, const int &to_ith_link) const
-{
-    _check_q_vec(q_vec);
-    _check_q_vec(q_vec_dot);
-    _check_to_ith_link(to_ith_link);
-
-    int n = to_ith_link+1;
-    DQ x_effector = raw_fkm(q_vec,to_ith_link);
-    MatrixXd J    = raw_pose_jacobian(q_vec,to_ith_link);
-    VectorXd vec_x_effector_dot = J*q_vec_dot.head(to_ith_link);
-
-    DQ x = DQ(1);
-    MatrixXd J_dot = MatrixXd::Zero(8,n);
-    int jth=0;
-
-    for(int i=0;i<n;i++)
-    {
-        const DQ w = _get_w(i);
-        const DQ z = 0.5*x*w*conj(x);
-
-        VectorXd vec_zdot;
-        if(i==0)
-        {
-            vec_zdot = VectorXd::Zero(8,1);
-        }
-        else
-        {
-            vec_zdot = 0.5*(haminus8(w*conj(x)) + hamiplus8(x*w)*C8())*raw_pose_jacobian(q_vec,i-1)*q_vec_dot.head(i);
-        }
-
-        J_dot.col(jth) = haminus8(x_effector)*vec_zdot + hamiplus8(z)*vec_x_effector_dot;
-        x = x*_dh2dq(q_vec(jth),i);
-        jth = jth+1;
-    }
-
-    return J_dot;
-}
-
-
-/**
- * @brief This method returns the pose Jacobian time derivative
- * @param q_vec. Vector of joint values.
- * @param q_vec_dot. Vector of joint velocity values.
- * @returns The pose jacobian derivative.
- *
- */
-MatrixXd DQ_SerialManipulatorDH::pose_jacobian_derivative(const VectorXd &q_vec, const VectorXd &q_vec_dot) const
-{
-    return pose_jacobian_derivative(q_vec, q_vec_dot, get_dim_configuration_space()-1);
-}
-
-
 /**
  * @brief This method calculates the forward kinematic model and returns the dual quaternion
  *        corresponding to the last joint (the displacements due to the base and the effector
@@ -306,5 +245,51 @@ MatrixXd DQ_SerialManipulatorDH::raw_pose_jacobian(const VectorXd &q_vec, const
     return J;
 }
 
+/**
+ * @brief This method returns the first to_ith_link columns of the time derivative of the pose Jacobian.
+ *        The base displacement and the effector are not taken into account.
+ * @param q_vec. Vector of joint values.
+ * @param q_vec_dot. Vector of joint velocity values.
+                  will be calculated.
+ * @param to_ith_link. The index to a link. This defines until which link the pose_jacobian_derivative
+ *                     will be calculated.
+ * @returns The first to_ith_link columns of the pose_jacobian_derivative.
+ *
+ */
+MatrixXd DQ_SerialManipulatorDH::raw_pose_jacobian_derivative(const VectorXd &q_vec, const VectorXd &q_vec_dot, const int &to_ith_link) const
+{
+    _check_q_vec(q_vec);
+    _check_q_vec(q_vec_dot);
+    _check_to_ith_link(to_ith_link);
+
+    int n = to_ith_link+1;
+    DQ x_effector = raw_fkm(q_vec,to_ith_link);
+    MatrixXd J    = raw_pose_jacobian(q_vec,to_ith_link);
+    VectorXd vec_x_effector_dot = J*q_vec_dot.head(n);
+    DQ x = DQ(1);
+    MatrixXd J_dot = MatrixXd::Zero(8,n);
+    int jth=0;
+
+    for(int i=0;i<n;i++)
+    {
+        const DQ w = _get_w(i);
+        const DQ z = 0.5*x*w*conj(x);
+
+        VectorXd vec_zdot;
+        if(i==0)
+        {
+            vec_zdot = VectorXd::Zero(8,1);
+        }
+        else
+        {
+            vec_zdot = 0.5*(haminus8(w*conj(x)) + hamiplus8(x*w)*C8())*raw_pose_jacobian(q_vec,i-1)*q_vec_dot.head(i);
+        }
+
+        J_dot.col(jth) = haminus8(x_effector)*vec_zdot + hamiplus8(z)*vec_x_effector_dot;
+        x = x*_dh2dq(q_vec(jth),i);
+        jth = jth+1;
+    }
 
+    return J_dot;
+}
 }

From b72c449d008cf131414a3e0e73d64a73f73f51ee Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Juan=20Jos=C3=A9=20Quiroz=20Oma=C3=B1a?=
 <juanjqogm@gmail.com>
Date: Fri, 11 Nov 2022 21:14:55 +0900
Subject: [PATCH 07/27] [DQ_SerialManipulatorMDH.h, .cpp] Added the
 pose_jacobian_derivative method.

---
 .../robot_modeling/DQ_SerialManipulatorMDH.h  |  5 ++--
 .../DQ_SerialManipulatorMDH.cpp               | 23 +++++--------------
 2 files changed, 8 insertions(+), 20 deletions(-)

diff --git a/include/dqrobotics/robot_modeling/DQ_SerialManipulatorMDH.h b/include/dqrobotics/robot_modeling/DQ_SerialManipulatorMDH.h
index c986425..3f3f66b 100644
--- a/include/dqrobotics/robot_modeling/DQ_SerialManipulatorMDH.h
+++ b/include/dqrobotics/robot_modeling/DQ_SerialManipulatorMDH.h
@@ -47,13 +47,12 @@ class DQ_SerialManipulatorMDH: public DQ_SerialManipulator
     DQ_SerialManipulatorMDH()=delete;
     DQ_SerialManipulatorMDH(const MatrixXd& mdh_matrix);    
 
-    MatrixXd pose_jacobian_derivative(const VectorXd& q_vec, const VectorXd& q_vec_dot, const int& to_ith_link) const;
-    MatrixXd pose_jacobian_derivative(const VectorXd& q_vec, const VectorXd& q_vec_dot) const;
-
     using DQ_SerialManipulator::raw_pose_jacobian;
+    using DQ_SerialManipulator::raw_pose_jacobian_derivative;
     using DQ_SerialManipulator::raw_fkm;
 
     MatrixXd raw_pose_jacobian(const VectorXd& q_vec, const int& to_ith_link) const override;
+    MatrixXd raw_pose_jacobian_derivative(const VectorXd& q_vec, const VectorXd& q_vec_dot, const int& to_ith_link) const override;
     DQ raw_fkm(const VectorXd &q_vec, const int &to_ith_link) const override;
 };
 
diff --git a/src/robot_modeling/DQ_SerialManipulatorMDH.cpp b/src/robot_modeling/DQ_SerialManipulatorMDH.cpp
index 2b83f30..ede0284 100644
--- a/src/robot_modeling/DQ_SerialManipulatorMDH.cpp
+++ b/src/robot_modeling/DQ_SerialManipulatorMDH.cpp
@@ -198,15 +198,17 @@ VectorXd DQ_SerialManipulatorMDH::get_types() const
 }
 
 /**
- * @brief This method returns the first to_ith_link columns of the pose Jacobian time derivative
+ * @brief This method returns the first to_ith_link columns of the time derivative of the pose Jacobian.
+ *        The base displacement and the effector are not taken into account.
  * @param q_vec. Vector of joint values.
  * @param q_vec_dot. Vector of joint velocity values.
+                  will be calculated.
  * @param to_ith_link. The index to a link. This defines until which link the pose_jacobian_derivative
  *                     will be calculated.
  * @returns The first to_ith_link columns of the pose_jacobian_derivative.
- * 
+ *
  */
-MatrixXd DQ_SerialManipulatorMDH::pose_jacobian_derivative(const VectorXd &q_vec, const VectorXd &q_vec_dot, const int &to_ith_link) const
+MatrixXd DQ_SerialManipulatorMDH::raw_pose_jacobian_derivative(const VectorXd &q_vec, const VectorXd &q_vec_dot, const int &to_ith_link) const
 {
     _check_q_vec(q_vec);
     _check_q_vec(q_vec_dot);
@@ -215,7 +217,7 @@ MatrixXd DQ_SerialManipulatorMDH::pose_jacobian_derivative(const VectorXd &q_vec
     int n = to_ith_link+1;
     DQ x_effector = raw_fkm(q_vec,to_ith_link);
     MatrixXd J    = raw_pose_jacobian(q_vec,to_ith_link);
-    VectorXd vec_x_effector_dot = J*q_vec_dot.head(to_ith_link);
+    VectorXd vec_x_effector_dot = J*q_vec_dot.head(n); //(to_ith_link);
 
     DQ x = DQ(1);
     MatrixXd J_dot = MatrixXd::Zero(8,n);
@@ -245,19 +247,6 @@ MatrixXd DQ_SerialManipulatorMDH::pose_jacobian_derivative(const VectorXd &q_vec
 }
 
 
-/**
- * @brief This method returns the pose Jacobian time derivative
- * @param q_vec. Vector of joint values.
- * @param q_vec_dot. Vector of joint velocity values. 
- * @returns The pose jacobian derivative.
- * 
- */
-MatrixXd DQ_SerialManipulatorMDH::pose_jacobian_derivative(const VectorXd &q_vec, const VectorXd &q_vec_dot) const
-{
-    return pose_jacobian_derivative(q_vec, q_vec_dot, get_dim_configuration_space()-1);
-}
-
-
 /**
  * @brief This method calculates the forward kinematic model and returns the dual quaternion
  *        corresponding to the last joint (the displacements due to the base and the effector

From e85a2f636012e2a09e3ca3887a0c92f1b6af5b41 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Juan=20Jos=C3=A9=20Quiroz=20Oma=C3=B1a?=
 <juanjqogm@gmail.com>
Date: Sun, 13 Nov 2022 19:25:40 +0900
Subject: [PATCH 08/27] [DQ_WholeBody.h, .cpp] Added the
 pose_jacobian_derivative method in the class WholeBody. However, the method
 is not implemented yet.

---
 .../dqrobotics/robot_modeling/DQ_WholeBody.h  |  5 ++++
 src/robot_modeling/DQ_WholeBody.cpp           | 29 +++++++++++++++++++
 2 files changed, 34 insertions(+)

diff --git a/include/dqrobotics/robot_modeling/DQ_WholeBody.h b/include/dqrobotics/robot_modeling/DQ_WholeBody.h
index c22bd06..f0bae17 100644
--- a/include/dqrobotics/robot_modeling/DQ_WholeBody.h
+++ b/include/dqrobotics/robot_modeling/DQ_WholeBody.h
@@ -55,6 +55,11 @@ class DQ_WholeBody : public DQ_Kinematics
     DQ fkm(const VectorXd&, const int& to_chain) const override;
     MatrixXd pose_jacobian(const VectorXd& q, const int& to_ith_chain) const override;
     MatrixXd pose_jacobian(const VectorXd& q) const override;
+    MatrixXd pose_jacobian_derivative(const VectorXd& q,
+                                      const VectorXd& q_dot,
+                                      const int& to_ith_link) const override; //To be implemented.
+    MatrixXd pose_jacobian_derivative (const VectorXd& q,
+                                       const VectorXd& q_dot) const override; //To be implemented.
 };
 
 }
diff --git a/src/robot_modeling/DQ_WholeBody.cpp b/src/robot_modeling/DQ_WholeBody.cpp
index f302edc..2b1de11 100644
--- a/src/robot_modeling/DQ_WholeBody.cpp
+++ b/src/robot_modeling/DQ_WholeBody.cpp
@@ -172,4 +172,33 @@ void DQ_WholeBody::set_effector(const DQ &effector)
     }
 }
 
+/**
+ * @brief returns the Jacobian derivative 'J_dot' that satisfies
+          vec8(pose_dot_dot) = J_dot * q_dot + J*q_dot_dot, where pose = fkm(), 'pose_dot' is the time
+          derivative of the pose and joint_configurations is the configuration vector.
+ * @param q The VectorXd representing the joint configurations.
+ * @param q_dot The VectorXd representing the joint velocity configurations.
+ * @param to_ith_link The 'to_ith_link' link which we want to compute the Jacobian derivative.
+ * @return a MatrixXd representing the desired Jacobian derivative.
+ */
+MatrixXd DQ_WholeBody::pose_jacobian_derivative(const VectorXd &q, const VectorXd &q_dot, const int &to_ith_link) const
+{
+    throw std::runtime_error(std::string("pose_jacobian_derivative is not implemented yet."));
+    return MatrixXd::Zero(1,1);
+}
+
+
+/**
+ * @brief returns the Jacobian derivative 'J_dot' that satisfies
+          vec8(pose_dot_dot) = J_dot * q_dot + J*q_dot_dot, where pose = fkm(), 'pose_dot' is the time
+          derivative of the pose and joint_configurations is the configuration vector.
+ * @param q The VectorXd representing the joint configurations.
+ * @param q_dot The VectorXd representing the joint velocity configurations.
+ * @return a MatrixXd representing the desired Jacobian derivative.
+ */
+MatrixXd DQ_WholeBody::pose_jacobian_derivative(const VectorXd &q, const VectorXd &q_dot) const
+{
+    return pose_jacobian_derivative(q,q_dot,  get_dim_configuration_space()-1);
+}
+
 }

From f77c28216c20aa0b801efda40d16388ac5142221 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Juan=20Jos=C3=A9=20Quiroz=20Oma=C3=B1a?=
 <juanjqogm@gmail.com>
Date: Mon, 14 Nov 2022 20:15:35 +0900
Subject: [PATCH 09/27] [DQ_Kinematics.h, .cpp] Renamed the variables
 joint_configurations and joint_velocity_configurations.

---
 include/dqrobotics/robot_modeling/DQ_Kinematics.h | 8 ++++----
 src/robot_modeling/DQ_Kinematics.cpp              | 4 ++--
 2 files changed, 6 insertions(+), 6 deletions(-)

diff --git a/include/dqrobotics/robot_modeling/DQ_Kinematics.h b/include/dqrobotics/robot_modeling/DQ_Kinematics.h
index 0e728bb..67cce29 100644
--- a/include/dqrobotics/robot_modeling/DQ_Kinematics.h
+++ b/include/dqrobotics/robot_modeling/DQ_Kinematics.h
@@ -64,13 +64,13 @@ class DQ_Kinematics
     virtual DQ fkm                (const VectorXd& joint_configurations) const = 0;
     virtual DQ fkm                (const VectorXd& joint_configurations, const int& to_ith_link) const = 0;
     virtual MatrixXd pose_jacobian(const VectorXd& joint_configurations, const int& to_ith_link) const = 0;
-    virtual MatrixXd pose_jacobian_derivative(const VectorXd& joint_configurations,
-                                              const VectorXd& joint_velocity_configurations,
+    virtual MatrixXd pose_jacobian_derivative(const VectorXd& configurations,
+                                              const VectorXd& velocity_configurations,
                                               const int& to_ith_link) const = 0;
     //Virtual methods
     virtual MatrixXd pose_jacobian (const VectorXd& joint_configurations) const;
-    virtual MatrixXd pose_jacobian_derivative(const VectorXd& joint_configurations,
-                                              const VectorXd& joint_velocity_configurations) const;
+    virtual MatrixXd pose_jacobian_derivative(const VectorXd& configurations,
+                                              const VectorXd& velocity_configurations) const;
     virtual int get_dim_configuration_space() const;
 
     //Static methods
diff --git a/src/robot_modeling/DQ_Kinematics.cpp b/src/robot_modeling/DQ_Kinematics.cpp
index 2f2323b..01ec8d2 100644
--- a/src/robot_modeling/DQ_Kinematics.cpp
+++ b/src/robot_modeling/DQ_Kinematics.cpp
@@ -139,9 +139,9 @@ MatrixXd DQ_Kinematics::pose_jacobian(const VectorXd &joint_configurations) cons
  * @param joint_velocity_configurations The VectorXd representing the joint velocity configurations.
  * @return a MatrixXd representing the desired Jacobian derivative.
  */
-MatrixXd DQ_Kinematics::pose_jacobian_derivative(const VectorXd &joint_configurations, const VectorXd &joint_velocity_configurations) const
+MatrixXd DQ_Kinematics::pose_jacobian_derivative(const VectorXd &configurations, const VectorXd &velocity_configurations) const
 {
-    return pose_jacobian_derivative(joint_configurations, joint_velocity_configurations, get_dim_configuration_space()-1);
+    return pose_jacobian_derivative(configurations, velocity_configurations, get_dim_configuration_space()-1);
 }
 
 /**

From f4174824f411d439df798b7b5abd8bd3264333c8 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Juan=20Jos=C3=A9=20Quiroz=20Oma=C3=B1a?=
 <juanjqogm@gmail.com>
Date: Mon, 14 Nov 2022 20:25:06 +0900
Subject: [PATCH 10/27] [DQ_Kinematics.cpp] Updated the documentation of the
 method pose_jacobian_derivative.

---
 src/robot_modeling/DQ_Kinematics.cpp | 4 ++--
 1 file changed, 2 insertions(+), 2 deletions(-)

diff --git a/src/robot_modeling/DQ_Kinematics.cpp b/src/robot_modeling/DQ_Kinematics.cpp
index 01ec8d2..0d2be74 100644
--- a/src/robot_modeling/DQ_Kinematics.cpp
+++ b/src/robot_modeling/DQ_Kinematics.cpp
@@ -135,8 +135,8 @@ MatrixXd DQ_Kinematics::pose_jacobian(const VectorXd &joint_configurations) cons
  * @brief returns the Jacobian derivative 'J_dot' that satisfies
           vec8(pose_dot_dot) = J_dot * q_dot + J*q_dot_dot, where pose = fkm(), 'pose_dot' is the time
           derivative of the pose and joint_configurations is the configuration vector.
- * @param joint_configurations The VectorXd representing the joint configurations.
- * @param joint_velocity_configurations The VectorXd representing the joint velocity configurations.
+ * @param configurations The VectorXd representing the joint configurations.
+ * @param velocity_configurations The VectorXd representing the joint velocity configurations.
  * @return a MatrixXd representing the desired Jacobian derivative.
  */
 MatrixXd DQ_Kinematics::pose_jacobian_derivative(const VectorXd &configurations, const VectorXd &velocity_configurations) const

From cf48dc04edcef7e09ac405ecdf2ae8323583862c Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Juan=20Jos=C3=A9=20Quiroz=20Oma=C3=B1a?=
 <juanjqogm@gmail.com>
Date: Mon, 14 Nov 2022 20:26:26 +0900
Subject: [PATCH 11/27] [DQ_DifferentialDriveRobot.h, .cpp] Renamed the
 variables joint_configurations and joint_velocity_configurations.

---
 .../DQ_DifferentialDriveRobot.h               |  4 ++--
 .../DQ_DifferentialDriveRobot.cpp             | 21 +++++++++++++------
 2 files changed, 17 insertions(+), 8 deletions(-)

diff --git a/include/dqrobotics/robot_modeling/DQ_DifferentialDriveRobot.h b/include/dqrobotics/robot_modeling/DQ_DifferentialDriveRobot.h
index 33d4988..1b418fd 100644
--- a/include/dqrobotics/robot_modeling/DQ_DifferentialDriveRobot.h
+++ b/include/dqrobotics/robot_modeling/DQ_DifferentialDriveRobot.h
@@ -41,8 +41,8 @@ class DQ_DifferentialDriveRobot : public DQ_HolonomicBase
     MatrixXd constraint_jacobian_derivative(const double& phi, const double& phi_dot) const;
     MatrixXd pose_jacobian(const VectorXd& q, const int& to_link) const override;
     MatrixXd pose_jacobian(const VectorXd &q) const override;
-    MatrixXd pose_jacobian_derivative(const VectorXd& q,
-                                      const VectorXd& q_dot,
+    MatrixXd pose_jacobian_derivative(const VectorXd& configurations,
+                                      const VectorXd& velocity_configurations,
                                       const int& to_link) const override;
     MatrixXd pose_jacobian_derivative (const VectorXd& q,
                                        const VectorXd& q_dot) const override;
diff --git a/src/robot_modeling/DQ_DifferentialDriveRobot.cpp b/src/robot_modeling/DQ_DifferentialDriveRobot.cpp
index 07406c5..8438af8 100644
--- a/src/robot_modeling/DQ_DifferentialDriveRobot.cpp
+++ b/src/robot_modeling/DQ_DifferentialDriveRobot.cpp
@@ -85,16 +85,21 @@ MatrixXd DQ_DifferentialDriveRobot::pose_jacobian(const VectorXd &q) const
 
 /**
  * @brief returns the pose Jacobian derivative
- * @param q The VectorXd representing the robot configuration.
- * @param q_dot The VectorXd representing the robot configuration velocity.
+ * @param configurations The VectorXd representing the robot configuration.
+ * @param velocity_configurations The VectorXd representing the robot configuration velocity.
  * @param to_link The ith link which we want to compute the Jacobian derivative.
  * @return a MatrixXd representing the desired Jacobian
  */
-MatrixXd DQ_DifferentialDriveRobot::pose_jacobian_derivative(const VectorXd &q, const VectorXd &q_dot, const int &to_link) const
+MatrixXd DQ_DifferentialDriveRobot::pose_jacobian_derivative(const VectorXd &configurations, const VectorXd &velocity_configurations, const int &to_link) const
 {
     if(to_link!=0 && to_link!=1)
         throw std::runtime_error("DQ_DifferentialDriveRobot::pose_jacobian_derivative(q,q_dot, to_link) only accepts to_link in {0,1}.");
 
+    /// Aliases
+    const VectorXd& q = configurations;
+    const VectorXd& q_dot = velocity_configurations;
+
+    /// Requirements
     MatrixXd J_holonomic = DQ_HolonomicBase::pose_jacobian(q,2);
     MatrixXd J_holonomic_dot = DQ_HolonomicBase::pose_jacobian_derivative(q,q_dot,2);
     MatrixXd J_c = constraint_jacobian(q(2));
@@ -105,15 +110,19 @@ MatrixXd DQ_DifferentialDriveRobot::pose_jacobian_derivative(const VectorXd &q,
 
 /**
  * @brief returns the pose Jacobian derivative
- * @param q The VectorXd representing the robot configuration.
- * @param q_dot The VectorXd representing the robot configuration velocity.
+ * @param configurations The VectorXd representing the robot configuration.
+ * @param velocity_configurations The VectorXd representing the robot configuration velocity.
  * @return a MatrixXd representing the desired Jacobian
  */
-MatrixXd DQ_DifferentialDriveRobot::pose_jacobian_derivative(const VectorXd &q, const VectorXd &q_dot) const
+MatrixXd DQ_DifferentialDriveRobot::pose_jacobian_derivative(const VectorXd &configurations, const VectorXd &velocity_configurations) const
 {
     // The DQ_DifferentialDriveRobot works differently from most other subclasses of DQ_Kinematics
     // The size of the configuration space is three but there is one constraint, so there are only
     // to columns in the pose_jacobian
+
+    /// Aliases
+    const VectorXd& q = configurations;
+    const VectorXd& q_dot = velocity_configurations;
     return pose_jacobian_derivative(q, q_dot, 1);
 }
 

From e7aa9c7677044c432b9a54e4757da8e8dfba640a Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Juan=20Jos=C3=A9=20Quiroz=20Oma=C3=B1a?=
 <juanjqogm@gmail.com>
Date: Mon, 14 Nov 2022 20:31:00 +0900
Subject: [PATCH 12/27] [DQ_HolonomicBase.h, .cpp] Renamed the variables
 joint_configurations and joint_velocity_configurations.

---
 .../robot_modeling/DQ_HolonomicBase.h         | 12 ++++----
 src/robot_modeling/DQ_HolonomicBase.cpp       | 29 +++++++++++++------
 2 files changed, 26 insertions(+), 15 deletions(-)

diff --git a/include/dqrobotics/robot_modeling/DQ_HolonomicBase.h b/include/dqrobotics/robot_modeling/DQ_HolonomicBase.h
index b3304f0..d4d868e 100644
--- a/include/dqrobotics/robot_modeling/DQ_HolonomicBase.h
+++ b/include/dqrobotics/robot_modeling/DQ_HolonomicBase.h
@@ -40,15 +40,15 @@ class DQ_HolonomicBase: public DQ_MobileBase
     virtual DQ fkm(const VectorXd& q, const int& to_ith_link) const override;
     virtual MatrixXd pose_jacobian(const VectorXd& q, const int& to_link) const override;
     virtual MatrixXd pose_jacobian(const VectorXd& q) const override;
-    virtual MatrixXd pose_jacobian_derivative(const VectorXd& q,
-                                              const VectorXd& q_dot, const int& to_link) const override;
-    virtual MatrixXd pose_jacobian_derivative(const VectorXd& q,
-                                              const VectorXd& q_dot) const override;
+    virtual MatrixXd pose_jacobian_derivative(const VectorXd& configurations,
+                                              const VectorXd& velocity_configurations, const int& to_link) const override;
+    virtual MatrixXd pose_jacobian_derivative(const VectorXd& configurations,
+                                              const VectorXd& velocity_configurations) const override;
 
     DQ raw_fkm(const VectorXd& q) const;
     MatrixXd raw_pose_jacobian(const VectorXd& q, const int& to_link=2) const;
-    MatrixXd raw_pose_jacobian_derivative(const VectorXd& q,
-                                          const VectorXd& q_dot, const int& to_link = 2) const;
+    MatrixXd raw_pose_jacobian_derivative(const VectorXd& configurations,
+                                          const VectorXd& velocity_configurations, const int& to_link = 2) const;
 };
 
 }
diff --git a/src/robot_modeling/DQ_HolonomicBase.cpp b/src/robot_modeling/DQ_HolonomicBase.cpp
index 2e75146..e359bba 100644
--- a/src/robot_modeling/DQ_HolonomicBase.cpp
+++ b/src/robot_modeling/DQ_HolonomicBase.cpp
@@ -112,17 +112,22 @@ MatrixXd DQ_HolonomicBase::pose_jacobian(const VectorXd &q) const
           vec8(pose_dot_dot) = J_dot * q_dot + J*q_dot_dot, where pose = fkm(), 'pose_dot' is the time
           derivative of the pose and  joint_configurations is the configuration vector.
           This method does not take into account the base displacement.
- * @param joint_configurations The VectorXd representing the joint configurations.
- * @param joint_velocity_configurations The VectorXd representing the joint velocity configurations.
+ * @param configurations The VectorXd representing the joint configurations.
+ * @param velocity_configurations The VectorXd representing the joint velocity configurations.
  * @param to_link The ith link which we want to compute the Jacobian derivative.
  * @return a MatrixXd representing the desired Jacobian derivative.
  */
-MatrixXd DQ_HolonomicBase::raw_pose_jacobian_derivative(const VectorXd &q, const VectorXd &q_dot, const int &to_link) const
+MatrixXd DQ_HolonomicBase::raw_pose_jacobian_derivative(const VectorXd &configurations, const VectorXd &velocity_configurations, const int &to_link) const
 {
     if(to_link >= 3 || to_link < 0)
     {
         throw std::runtime_error(std::string("Tried to access link index ") + std::to_string(to_link) + std::string(" which is unnavailable."));
     }
+
+    /// Aliases
+    const VectorXd& q = configurations;
+    const VectorXd& q_dot = velocity_configurations;
+
     const double& x   = q(0);
     const double& y   = q(1);
     const double& phi = q(2);
@@ -161,13 +166,16 @@ MatrixXd DQ_HolonomicBase::raw_pose_jacobian_derivative(const VectorXd &q, const
  * @brief returns the Jacobian derivative (J_dot) that satisfies
           vec8(pose_dot_dot) = J_dot * q_dot + J*q_dot_dot, where pose = fkm(), 'pose_dot' is the time
           derivative of the pose and  joint_configurations is the configuration vector.
- * @param joint_configurations The VectorXd representing the joint configurations.
- * @param joint_velocity_configurations The VectorXd representing the joint velocity configurations.
+ * @param configurations The VectorXd representing the joint configurations.
+ * @param velocity_configurations The VectorXd representing the joint velocity configurations.
  * @param to_link The ith link which we want to compute the Jacobian derivative.
  * @return a MatrixXd representing the desired Jacobian derivative.
  */
-MatrixXd DQ_HolonomicBase::pose_jacobian_derivative(const VectorXd &q, const VectorXd &q_dot, const int &to_link) const
+MatrixXd DQ_HolonomicBase::pose_jacobian_derivative(const VectorXd &configurations, const VectorXd &velocity_configurations, const int &to_link) const
 {
+    /// Aliases
+    const VectorXd& q = configurations;
+    const VectorXd& q_dot = velocity_configurations;
     return haminus8(frame_displacement_)*raw_pose_jacobian_derivative(q, q_dot, to_link);
 }
 
@@ -175,12 +183,15 @@ MatrixXd DQ_HolonomicBase::pose_jacobian_derivative(const VectorXd &q, const Vec
  * @brief returns the Jacobian derivative (J_dot) that satisfies
           vec8(pose_dot_dot) = J_dot * q_dot + J*q_dot_dot, where pose = fkm(), 'pose_dot' is the time
           derivative of the pose and  joint_configurations is the configuration vector.
- * @param joint_configurations The VectorXd representing the joint configurations.
- * @param joint_velocity_configurations The VectorXd representing the joint velocity configurations.
+ * @param configurations The VectorXd representing the joint configurations.
+ * @param velocity_configurations The VectorXd representing the joint velocity configurations.
  * @return a MatrixXd representing the desired Jacobian derivative.
  */
-MatrixXd DQ_HolonomicBase::pose_jacobian_derivative(const VectorXd &q, const VectorXd &q_dot) const
+MatrixXd DQ_HolonomicBase::pose_jacobian_derivative(const VectorXd &configurations, const VectorXd &velocity_configurations) const
 {
+    /// Aliases
+    const VectorXd& q = configurations;
+    const VectorXd& q_dot = velocity_configurations;
     return pose_jacobian_derivative(q, q_dot, get_dim_configuration_space()-1);
 }
 

From fe8fcef75397a1d72601ec4564c47dbe012bf3f2 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Juan=20Jos=C3=A9=20Quiroz=20Oma=C3=B1a?=
 <juanjqogm@gmail.com>
Date: Mon, 14 Nov 2022 20:34:34 +0900
Subject: [PATCH 13/27] [DQ_MobileBase.h] Added a comment about the virtual
 method pose_jacobian_derivative.

---
 include/dqrobotics/robot_modeling/DQ_MobileBase.h | 1 +
 1 file changed, 1 insertion(+)

diff --git a/include/dqrobotics/robot_modeling/DQ_MobileBase.h b/include/dqrobotics/robot_modeling/DQ_MobileBase.h
index 3ad9295..1e29843 100644
--- a/include/dqrobotics/robot_modeling/DQ_MobileBase.h
+++ b/include/dqrobotics/robot_modeling/DQ_MobileBase.h
@@ -42,6 +42,7 @@ class DQ_MobileBase : public DQ_Kinematics
     //virtual int      get_dim_configuration_space() const = 0;
     //virtual DQ       fkm(const VectorXd& joint_configurations) const = 0;
     //virtual MatrixXd pose_jacobian(const VectorXd& joint_configurations,const int& to_link) const = 0;
+    //virtual MatrixXd pose_jacobian_derivative(const VectorXd& configurations, const VectorXd& velocity_configurations, const int& to_link) const = 0;
 
     void set_frame_displacement(const DQ& pose);
     DQ   frame_displacement();

From c1bcb3a3279499b049176032fd2ba3f415e08a35 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Juan=20Jos=C3=A9=20Quiroz=20Oma=C3=B1a?=
 <juanjqogm@gmail.com>
Date: Mon, 14 Nov 2022 20:42:58 +0900
Subject: [PATCH 14/27] [DQ_SerialWholeBody.h, .cpp] Renamed the variables
 joint_configurations and joint_velocity_configurations.

---
 .../robot_modeling/DQ_SerialWholeBody.h       | 12 ++++-----
 src/robot_modeling/DQ_SerialWholeBody.cpp     | 26 ++++++++++++-------
 2 files changed, 23 insertions(+), 15 deletions(-)

diff --git a/include/dqrobotics/robot_modeling/DQ_SerialWholeBody.h b/include/dqrobotics/robot_modeling/DQ_SerialWholeBody.h
index 9721ce9..ca69729 100644
--- a/include/dqrobotics/robot_modeling/DQ_SerialWholeBody.h
+++ b/include/dqrobotics/robot_modeling/DQ_SerialWholeBody.h
@@ -58,8 +58,8 @@ class DQ_SerialWholeBody : public DQ_Kinematics
     DQ_SerialManipulatorDH get_chain_as_serial_manipulator_dh(const int& to_ith_chain) const;
     DQ_HolonomicBase get_chain_as_holonomic_base(const int& to_ith_chain) const;
     MatrixXd raw_pose_jacobian_by_chain(const VectorXd& q, const int& to_ith_chain, const int& to_jth_link) const;
-    MatrixXd raw_pose_jacobian_derivative_by_chain(const VectorXd& q,
-                                                   const VectorXd& q_dot,
+    MatrixXd raw_pose_jacobian_derivative_by_chain(const VectorXd& configurations,
+                                                   const VectorXd& velocity_configurations,
                                                    const int& to_ith_chain,
                                                    const int& to_jth_link) const; //To be implemented.
 
@@ -68,11 +68,11 @@ class DQ_SerialWholeBody : public DQ_Kinematics
     DQ fkm(const VectorXd&, const int& to_ith_link) const override;
     MatrixXd pose_jacobian(const VectorXd& q, const int& to_ith_link) const override;
     MatrixXd pose_jacobian(const VectorXd& q) const override;
-    MatrixXd pose_jacobian_derivative(const VectorXd& q,
-                                      const VectorXd& q_dot,
+    MatrixXd pose_jacobian_derivative(const VectorXd& configurations,
+                                      const VectorXd& velocity_configurations,
                                       const int& to_ith_link) const override; //To be implemented.
-    MatrixXd pose_jacobian_derivative (const VectorXd& q,
-                                       const VectorXd& q_dot) const override; //To be implemented.
+    MatrixXd pose_jacobian_derivative (const VectorXd& configurations,
+                                       const VectorXd& velocity_configurations) const override; //To be implemented.
 };
 
 }
diff --git a/src/robot_modeling/DQ_SerialWholeBody.cpp b/src/robot_modeling/DQ_SerialWholeBody.cpp
index 44f8884..74707c7 100644
--- a/src/robot_modeling/DQ_SerialWholeBody.cpp
+++ b/src/robot_modeling/DQ_SerialWholeBody.cpp
@@ -251,7 +251,7 @@ MatrixXd DQ_SerialWholeBody::pose_jacobian(const VectorXd &q) const
 }
 
 //To be implemented.
-MatrixXd DQ_SerialWholeBody::raw_pose_jacobian_derivative_by_chain(const VectorXd &q, const VectorXd &q_dot, const int &to_ith_chain, const int &to_jth_link) const
+MatrixXd DQ_SerialWholeBody::raw_pose_jacobian_derivative_by_chain(const VectorXd &configurations, const VectorXd &velocity_configurations, const int &to_ith_chain, const int &to_jth_link) const
 {
     throw std::runtime_error(std::string("pose_jacobian_derivative_by_chain is not implemented yet."));
     return MatrixXd::Zero(1,1);
@@ -260,16 +260,21 @@ MatrixXd DQ_SerialWholeBody::raw_pose_jacobian_derivative_by_chain(const VectorX
 /**
  * @brief returns the Jacobian derivative 'J_dot' that satisfies
           vec8(pose_dot_dot) = J_dot * q_dot + J*q_dot_dot, where pose = fkm(), 'pose_dot' is the time
-          derivative of the pose and joint_configurations is the configuration vector.
- * @param q The VectorXd representing the joint configurations.
- * @param q_dot The VectorXd representing the joint velocity configurations.
+          derivative of the pose and 'q_dot' represents the robot velocity configurations.
+ * @param configurations The VectorXd representing the robot configurations.
+ * @param velocity_configurations The VectorXd representing the robot velocity configurations.
  * @param to_ith_link The 'to_ith_link' link which we want to compute the Jacobian derivative.
  * @return a MatrixXd representing the desired Jacobian derivative.
  */
-MatrixXd DQ_SerialWholeBody::pose_jacobian_derivative(const VectorXd &q, const VectorXd &q_dot, const int &to_ith_link) const
+MatrixXd DQ_SerialWholeBody::pose_jacobian_derivative(const VectorXd &configurations, const VectorXd &velocity_configurations, const int &to_ith_link) const
 {
     int to_ith_chain;
     int to_jth_link;
+
+    /// Aliases
+    const VectorXd& q = configurations;
+    const VectorXd& q_dot = velocity_configurations;
+
     std::tie(to_ith_chain,to_jth_link) = get_chain_and_link_from_index(to_ith_link);
     return raw_pose_jacobian_derivative_by_chain(q, q_dot, to_ith_chain,to_ith_link);
 }
@@ -278,13 +283,16 @@ MatrixXd DQ_SerialWholeBody::pose_jacobian_derivative(const VectorXd &q, const V
 /**
  * @brief returns the Jacobian derivative 'J_dot' that satisfies
           vec8(pose_dot_dot) = J_dot * q_dot + J*q_dot_dot, where pose = fkm(), 'pose_dot' is the time
-          derivative of the pose and joint_configurations is the configuration vector.
- * @param q The VectorXd representing the joint configurations.
- * @param q_dot The VectorXd representing the joint velocity configurations.
+          derivative of the pose and 'q_dot' represents the robot velocity configurations.
+ * @param configurations The VectorXd representing the robot configurations.
+ * @param velocity_configurations The VectorXd representing the robot velocity configurations.
  * @return a MatrixXd representing the desired Jacobian derivative.
  */
-MatrixXd DQ_SerialWholeBody::pose_jacobian_derivative(const VectorXd &q, const VectorXd &q_dot) const
+MatrixXd DQ_SerialWholeBody::pose_jacobian_derivative(const VectorXd &configurations, const VectorXd &velocity_configurations) const
 {
+    /// Aliases
+    const VectorXd& q = configurations;
+    const VectorXd& q_dot = velocity_configurations;
     return pose_jacobian_derivative(q,q_dot,  get_dim_configuration_space()-1);
 }
 

From 69c41b78f6b448c65d6f8eb7809da8975bef0edd Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Juan=20Jos=C3=A9=20Quiroz=20Oma=C3=B1a?=
 <juanjqogm@gmail.com>
Date: Mon, 14 Nov 2022 20:46:31 +0900
Subject: [PATCH 15/27] [DQ_WholeBody.h, .cpp] Renamed the variables
 joint_configurations and joint_velocity_configurations.

---
 .../dqrobotics/robot_modeling/DQ_WholeBody.h  |  8 ++++----
 src/robot_modeling/DQ_WholeBody.cpp           | 20 +++++++++++--------
 2 files changed, 16 insertions(+), 12 deletions(-)

diff --git a/include/dqrobotics/robot_modeling/DQ_WholeBody.h b/include/dqrobotics/robot_modeling/DQ_WholeBody.h
index f0bae17..d69a218 100644
--- a/include/dqrobotics/robot_modeling/DQ_WholeBody.h
+++ b/include/dqrobotics/robot_modeling/DQ_WholeBody.h
@@ -55,11 +55,11 @@ class DQ_WholeBody : public DQ_Kinematics
     DQ fkm(const VectorXd&, const int& to_chain) const override;
     MatrixXd pose_jacobian(const VectorXd& q, const int& to_ith_chain) const override;
     MatrixXd pose_jacobian(const VectorXd& q) const override;
-    MatrixXd pose_jacobian_derivative(const VectorXd& q,
-                                      const VectorXd& q_dot,
+    MatrixXd pose_jacobian_derivative(const VectorXd& configurations,
+                                      const VectorXd& velocity_configurations,
                                       const int& to_ith_link) const override; //To be implemented.
-    MatrixXd pose_jacobian_derivative (const VectorXd& q,
-                                       const VectorXd& q_dot) const override; //To be implemented.
+    MatrixXd pose_jacobian_derivative (const VectorXd& configurations,
+                                       const VectorXd& velocity_configurations) const override; //To be implemented.
 };
 
 }
diff --git a/src/robot_modeling/DQ_WholeBody.cpp b/src/robot_modeling/DQ_WholeBody.cpp
index 2b1de11..9e53235 100644
--- a/src/robot_modeling/DQ_WholeBody.cpp
+++ b/src/robot_modeling/DQ_WholeBody.cpp
@@ -175,13 +175,13 @@ void DQ_WholeBody::set_effector(const DQ &effector)
 /**
  * @brief returns the Jacobian derivative 'J_dot' that satisfies
           vec8(pose_dot_dot) = J_dot * q_dot + J*q_dot_dot, where pose = fkm(), 'pose_dot' is the time
-          derivative of the pose and joint_configurations is the configuration vector.
- * @param q The VectorXd representing the joint configurations.
- * @param q_dot The VectorXd representing the joint velocity configurations.
+          derivative of the pose and 'q_dot' represents the robot velocity configurations.
+ * @param configurations The VectorXd representing the robot configurations.
+ * @param velocity_configurations The VectorXd representing the robot velocity configurations.
  * @param to_ith_link The 'to_ith_link' link which we want to compute the Jacobian derivative.
  * @return a MatrixXd representing the desired Jacobian derivative.
  */
-MatrixXd DQ_WholeBody::pose_jacobian_derivative(const VectorXd &q, const VectorXd &q_dot, const int &to_ith_link) const
+MatrixXd DQ_WholeBody::pose_jacobian_derivative(const VectorXd &configurations, const VectorXd &velocity_configurations, const int &to_ith_link) const
 {
     throw std::runtime_error(std::string("pose_jacobian_derivative is not implemented yet."));
     return MatrixXd::Zero(1,1);
@@ -191,13 +191,17 @@ MatrixXd DQ_WholeBody::pose_jacobian_derivative(const VectorXd &q, const VectorX
 /**
  * @brief returns the Jacobian derivative 'J_dot' that satisfies
           vec8(pose_dot_dot) = J_dot * q_dot + J*q_dot_dot, where pose = fkm(), 'pose_dot' is the time
-          derivative of the pose and joint_configurations is the configuration vector.
- * @param q The VectorXd representing the joint configurations.
- * @param q_dot The VectorXd representing the joint velocity configurations.
+          derivative of the pose and 'q_dot' represents the robot velocity configurations.
+ * @param configurations The VectorXd representing the robot configurations.
+ * @param velocity_configurations The VectorXd representing the robot velocity configurations.
  * @return a MatrixXd representing the desired Jacobian derivative.
  */
-MatrixXd DQ_WholeBody::pose_jacobian_derivative(const VectorXd &q, const VectorXd &q_dot) const
+MatrixXd DQ_WholeBody::pose_jacobian_derivative(const VectorXd &configurations, const VectorXd &velocity_configurations) const
 {
+    /// Aliases
+    const VectorXd& q = configurations;
+    const VectorXd& q_dot = velocity_configurations;
+
     return pose_jacobian_derivative(q,q_dot,  get_dim_configuration_space()-1);
 }
 

From 06218c7ad22340245cf7c91d910d9a918cb2cdb6 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Juan=20Jos=C3=A9=20Quiroz=20Oma=C3=B1a?=
 <juanjqogm@gmail.com>
Date: Mon, 14 Nov 2022 20:48:11 +0900
Subject: [PATCH 16/27] [DQ_Kinematics.cpp] Updated the documentation of the
 pose_jacobian_derivative() method.

---
 src/robot_modeling/DQ_Kinematics.cpp | 6 +++---
 1 file changed, 3 insertions(+), 3 deletions(-)

diff --git a/src/robot_modeling/DQ_Kinematics.cpp b/src/robot_modeling/DQ_Kinematics.cpp
index 0d2be74..7a5ebce 100644
--- a/src/robot_modeling/DQ_Kinematics.cpp
+++ b/src/robot_modeling/DQ_Kinematics.cpp
@@ -134,9 +134,9 @@ MatrixXd DQ_Kinematics::pose_jacobian(const VectorXd &joint_configurations) cons
 /**
  * @brief returns the Jacobian derivative 'J_dot' that satisfies
           vec8(pose_dot_dot) = J_dot * q_dot + J*q_dot_dot, where pose = fkm(), 'pose_dot' is the time
-          derivative of the pose and joint_configurations is the configuration vector.
- * @param configurations The VectorXd representing the joint configurations.
- * @param velocity_configurations The VectorXd representing the joint velocity configurations.
+          derivative of the pose and 'q_dot' represents the robot velocity configurations.
+ * @param configurations The VectorXd representing the robot configurations.
+ * @param velocity_configurations The VectorXd representing the robot velocity configurations.
  * @return a MatrixXd representing the desired Jacobian derivative.
  */
 MatrixXd DQ_Kinematics::pose_jacobian_derivative(const VectorXd &configurations, const VectorXd &velocity_configurations) const

From 8eccfbbc877bf0a9a921b3128e0f00ba4328f4dd Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Juan=20Jos=C3=A9=20Quiroz=20Oma=C3=B1a?=
 <juanjqogm@gmail.com>
Date: Mon, 14 Nov 2022 20:49:59 +0900
Subject: [PATCH 17/27] [DQ_HolonomicBase.cpp] Updated the documentation of the
 pose_jacobian_derivative() method.

---
 src/robot_modeling/DQ_HolonomicBase.cpp | 18 +++++++++---------
 1 file changed, 9 insertions(+), 9 deletions(-)

diff --git a/src/robot_modeling/DQ_HolonomicBase.cpp b/src/robot_modeling/DQ_HolonomicBase.cpp
index e359bba..a4f89a1 100644
--- a/src/robot_modeling/DQ_HolonomicBase.cpp
+++ b/src/robot_modeling/DQ_HolonomicBase.cpp
@@ -163,12 +163,12 @@ MatrixXd DQ_HolonomicBase::raw_pose_jacobian_derivative(const VectorXd &configur
 }
 
 /**
- * @brief returns the Jacobian derivative (J_dot) that satisfies
+ * @brief returns the Jacobian derivative 'J_dot' that satisfies
           vec8(pose_dot_dot) = J_dot * q_dot + J*q_dot_dot, where pose = fkm(), 'pose_dot' is the time
-          derivative of the pose and  joint_configurations is the configuration vector.
- * @param configurations The VectorXd representing the joint configurations.
- * @param velocity_configurations The VectorXd representing the joint velocity configurations.
- * @param to_link The ith link which we want to compute the Jacobian derivative.
+          derivative of the pose and 'q_dot' represents the robot velocity configurations.
+ * @param configurations The VectorXd representing the robot configurations.
+ * @param velocity_configurations The VectorXd representing the robot velocity configurations.
+ * @param to_ith_link The 'to_ith_link' link which we want to compute the Jacobian derivative.
  * @return a MatrixXd representing the desired Jacobian derivative.
  */
 MatrixXd DQ_HolonomicBase::pose_jacobian_derivative(const VectorXd &configurations, const VectorXd &velocity_configurations, const int &to_link) const
@@ -180,11 +180,11 @@ MatrixXd DQ_HolonomicBase::pose_jacobian_derivative(const VectorXd &configuratio
 }
 
 /**
- * @brief returns the Jacobian derivative (J_dot) that satisfies
+ * @brief returns the Jacobian derivative 'J_dot' that satisfies
           vec8(pose_dot_dot) = J_dot * q_dot + J*q_dot_dot, where pose = fkm(), 'pose_dot' is the time
-          derivative of the pose and  joint_configurations is the configuration vector.
- * @param configurations The VectorXd representing the joint configurations.
- * @param velocity_configurations The VectorXd representing the joint velocity configurations.
+          derivative of the pose and 'q_dot' represents the robot velocity configurations.
+ * @param configurations The VectorXd representing the robot configurations.
+ * @param velocity_configurations The VectorXd representing the robot velocity configurations.
  * @return a MatrixXd representing the desired Jacobian derivative.
  */
 MatrixXd DQ_HolonomicBase::pose_jacobian_derivative(const VectorXd &configurations, const VectorXd &velocity_configurations) const

From 24796c516fc8bb934c4154472d766cb050b3583b Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Juan=20Jos=C3=A9=20Quiroz=20Oma=C3=B1a?=
 <juanjqogm@gmail.com>
Date: Mon, 14 Nov 2022 21:01:58 +0900
Subject: [PATCH 18/27] [DQ_DifferentialDriveRobot.h, .cpp] Renamed the
 variables joint_configurations and joint_velocity_configurations. I had
 forgotten to make the changes in one of the methods.

---
 include/dqrobotics/robot_modeling/DQ_DifferentialDriveRobot.h | 4 ++--
 1 file changed, 2 insertions(+), 2 deletions(-)

diff --git a/include/dqrobotics/robot_modeling/DQ_DifferentialDriveRobot.h b/include/dqrobotics/robot_modeling/DQ_DifferentialDriveRobot.h
index 1b418fd..efb83f9 100644
--- a/include/dqrobotics/robot_modeling/DQ_DifferentialDriveRobot.h
+++ b/include/dqrobotics/robot_modeling/DQ_DifferentialDriveRobot.h
@@ -44,8 +44,8 @@ class DQ_DifferentialDriveRobot : public DQ_HolonomicBase
     MatrixXd pose_jacobian_derivative(const VectorXd& configurations,
                                       const VectorXd& velocity_configurations,
                                       const int& to_link) const override;
-    MatrixXd pose_jacobian_derivative (const VectorXd& q,
-                                       const VectorXd& q_dot) const override;
+    MatrixXd pose_jacobian_derivative (const VectorXd& configurations,
+                                       const VectorXd& velocity_configurations) const override;
 };
 
 }

From 504674fee102460ad2e875294d7c176332852f3c Mon Sep 17 00:00:00 2001
From: Juancho <juanjqogm@gmail.com>
Date: Wed, 16 Nov 2022 16:40:09 +0900
Subject: [PATCH 19/27] [DQ_Kinematics.h, .cpp] Renamed the variables of
 pose_jacobian_derivative

---
 include/dqrobotics/robot_modeling/DQ_Kinematics.h |  8 ++++----
 src/robot_modeling/DQ_Kinematics.cpp              | 10 +++++-----
 2 files changed, 9 insertions(+), 9 deletions(-)

diff --git a/include/dqrobotics/robot_modeling/DQ_Kinematics.h b/include/dqrobotics/robot_modeling/DQ_Kinematics.h
index 67cce29..d85b883 100644
--- a/include/dqrobotics/robot_modeling/DQ_Kinematics.h
+++ b/include/dqrobotics/robot_modeling/DQ_Kinematics.h
@@ -64,13 +64,13 @@ class DQ_Kinematics
     virtual DQ fkm                (const VectorXd& joint_configurations) const = 0;
     virtual DQ fkm                (const VectorXd& joint_configurations, const int& to_ith_link) const = 0;
     virtual MatrixXd pose_jacobian(const VectorXd& joint_configurations, const int& to_ith_link) const = 0;
-    virtual MatrixXd pose_jacobian_derivative(const VectorXd& configurations,
-                                              const VectorXd& velocity_configurations,
+    virtual MatrixXd pose_jacobian_derivative(const VectorXd& q,
+                                              const VectorXd& q_dot,
                                               const int& to_ith_link) const = 0;
     //Virtual methods
     virtual MatrixXd pose_jacobian (const VectorXd& joint_configurations) const;
-    virtual MatrixXd pose_jacobian_derivative(const VectorXd& configurations,
-                                              const VectorXd& velocity_configurations) const;
+    virtual MatrixXd pose_jacobian_derivative(const VectorXd& q,
+                                              const VectorXd& q_dot) const;
     virtual int get_dim_configuration_space() const;
 
     //Static methods
diff --git a/src/robot_modeling/DQ_Kinematics.cpp b/src/robot_modeling/DQ_Kinematics.cpp
index 7a5ebce..f4a1b26 100644
--- a/src/robot_modeling/DQ_Kinematics.cpp
+++ b/src/robot_modeling/DQ_Kinematics.cpp
@@ -134,14 +134,14 @@ MatrixXd DQ_Kinematics::pose_jacobian(const VectorXd &joint_configurations) cons
 /**
  * @brief returns the Jacobian derivative 'J_dot' that satisfies
           vec8(pose_dot_dot) = J_dot * q_dot + J*q_dot_dot, where pose = fkm(), 'pose_dot' is the time
-          derivative of the pose and 'q_dot' represents the robot velocity configurations.
- * @param configurations The VectorXd representing the robot configurations.
- * @param velocity_configurations The VectorXd representing the robot velocity configurations.
+          derivative of the pose and 'q_dot' represents the robot configuration velocities.
+ * @param q The VectorXd representing the robot configurations.
+ * @param q_dot The VectorXd representing the robot configuration velocities.
  * @return a MatrixXd representing the desired Jacobian derivative.
  */
-MatrixXd DQ_Kinematics::pose_jacobian_derivative(const VectorXd &configurations, const VectorXd &velocity_configurations) const
+MatrixXd DQ_Kinematics::pose_jacobian_derivative(const VectorXd &q, const VectorXd &q_dot) const
 {
-    return pose_jacobian_derivative(configurations, velocity_configurations, get_dim_configuration_space()-1);
+    return pose_jacobian_derivative(q, q_dot, get_dim_configuration_space()-1);
 }
 
 /**

From 1148048ccb085a8c10590ab18cf266a0a04076db Mon Sep 17 00:00:00 2001
From: Juancho <juanjqogm@gmail.com>
Date: Wed, 16 Nov 2022 16:54:48 +0900
Subject: [PATCH 20/27] [DQ_DifferentialDriveRobot.h, .cpp] Renamed the
 variables of pose_jacobian_derivative

---
 .../DQ_DifferentialDriveRobot.h               |  8 ++++----
 .../DQ_DifferentialDriveRobot.cpp             | 20 ++++++-------------
 2 files changed, 10 insertions(+), 18 deletions(-)

diff --git a/include/dqrobotics/robot_modeling/DQ_DifferentialDriveRobot.h b/include/dqrobotics/robot_modeling/DQ_DifferentialDriveRobot.h
index efb83f9..33d4988 100644
--- a/include/dqrobotics/robot_modeling/DQ_DifferentialDriveRobot.h
+++ b/include/dqrobotics/robot_modeling/DQ_DifferentialDriveRobot.h
@@ -41,11 +41,11 @@ class DQ_DifferentialDriveRobot : public DQ_HolonomicBase
     MatrixXd constraint_jacobian_derivative(const double& phi, const double& phi_dot) const;
     MatrixXd pose_jacobian(const VectorXd& q, const int& to_link) const override;
     MatrixXd pose_jacobian(const VectorXd &q) const override;
-    MatrixXd pose_jacobian_derivative(const VectorXd& configurations,
-                                      const VectorXd& velocity_configurations,
+    MatrixXd pose_jacobian_derivative(const VectorXd& q,
+                                      const VectorXd& q_dot,
                                       const int& to_link) const override;
-    MatrixXd pose_jacobian_derivative (const VectorXd& configurations,
-                                       const VectorXd& velocity_configurations) const override;
+    MatrixXd pose_jacobian_derivative (const VectorXd& q,
+                                       const VectorXd& q_dot) const override;
 };
 
 }
diff --git a/src/robot_modeling/DQ_DifferentialDriveRobot.cpp b/src/robot_modeling/DQ_DifferentialDriveRobot.cpp
index 8438af8..bb1be09 100644
--- a/src/robot_modeling/DQ_DifferentialDriveRobot.cpp
+++ b/src/robot_modeling/DQ_DifferentialDriveRobot.cpp
@@ -85,20 +85,16 @@ MatrixXd DQ_DifferentialDriveRobot::pose_jacobian(const VectorXd &q) const
 
 /**
  * @brief returns the pose Jacobian derivative
- * @param configurations The VectorXd representing the robot configuration.
- * @param velocity_configurations The VectorXd representing the robot configuration velocity.
+ * @param q The VectorXd representing the robot configuration.
+ * @param q_dot The VectorXd representing the robot configuration velocities.
  * @param to_link The ith link which we want to compute the Jacobian derivative.
  * @return a MatrixXd representing the desired Jacobian
  */
-MatrixXd DQ_DifferentialDriveRobot::pose_jacobian_derivative(const VectorXd &configurations, const VectorXd &velocity_configurations, const int &to_link) const
+MatrixXd DQ_DifferentialDriveRobot::pose_jacobian_derivative(const VectorXd &q, const VectorXd &q_dot, const int &to_link) const
 {
     if(to_link!=0 && to_link!=1)
         throw std::runtime_error("DQ_DifferentialDriveRobot::pose_jacobian_derivative(q,q_dot, to_link) only accepts to_link in {0,1}.");
 
-    /// Aliases
-    const VectorXd& q = configurations;
-    const VectorXd& q_dot = velocity_configurations;
-
     /// Requirements
     MatrixXd J_holonomic = DQ_HolonomicBase::pose_jacobian(q,2);
     MatrixXd J_holonomic_dot = DQ_HolonomicBase::pose_jacobian_derivative(q,q_dot,2);
@@ -110,19 +106,15 @@ MatrixXd DQ_DifferentialDriveRobot::pose_jacobian_derivative(const VectorXd &con
 
 /**
  * @brief returns the pose Jacobian derivative
- * @param configurations The VectorXd representing the robot configuration.
- * @param velocity_configurations The VectorXd representing the robot configuration velocity.
+ * @param q The VectorXd representing the robot configuration.
+ * @param q_dot The VectorXd representing the robot configuration velocities.
  * @return a MatrixXd representing the desired Jacobian
  */
-MatrixXd DQ_DifferentialDriveRobot::pose_jacobian_derivative(const VectorXd &configurations, const VectorXd &velocity_configurations) const
+MatrixXd DQ_DifferentialDriveRobot::pose_jacobian_derivative(const VectorXd &q, const VectorXd &q_dot) const
 {
     // The DQ_DifferentialDriveRobot works differently from most other subclasses of DQ_Kinematics
     // The size of the configuration space is three but there is one constraint, so there are only
     // to columns in the pose_jacobian
-
-    /// Aliases
-    const VectorXd& q = configurations;
-    const VectorXd& q_dot = velocity_configurations;
     return pose_jacobian_derivative(q, q_dot, 1);
 }
 

From 77edacc892977bcc89e8712ed67b7c8f167a41ba Mon Sep 17 00:00:00 2001
From: Juancho <juanjqogm@gmail.com>
Date: Wed, 16 Nov 2022 17:02:43 +0900
Subject: [PATCH 21/27] [DQ_HolonomicBase.h, .cpp] Renamed the variables of
 pose_jacobian_derivative

---
 .../robot_modeling/DQ_HolonomicBase.h         | 12 +++----
 src/robot_modeling/DQ_HolonomicBase.cpp       | 34 +++++++------------
 2 files changed, 18 insertions(+), 28 deletions(-)

diff --git a/include/dqrobotics/robot_modeling/DQ_HolonomicBase.h b/include/dqrobotics/robot_modeling/DQ_HolonomicBase.h
index d4d868e..b3304f0 100644
--- a/include/dqrobotics/robot_modeling/DQ_HolonomicBase.h
+++ b/include/dqrobotics/robot_modeling/DQ_HolonomicBase.h
@@ -40,15 +40,15 @@ class DQ_HolonomicBase: public DQ_MobileBase
     virtual DQ fkm(const VectorXd& q, const int& to_ith_link) const override;
     virtual MatrixXd pose_jacobian(const VectorXd& q, const int& to_link) const override;
     virtual MatrixXd pose_jacobian(const VectorXd& q) const override;
-    virtual MatrixXd pose_jacobian_derivative(const VectorXd& configurations,
-                                              const VectorXd& velocity_configurations, const int& to_link) const override;
-    virtual MatrixXd pose_jacobian_derivative(const VectorXd& configurations,
-                                              const VectorXd& velocity_configurations) const override;
+    virtual MatrixXd pose_jacobian_derivative(const VectorXd& q,
+                                              const VectorXd& q_dot, const int& to_link) const override;
+    virtual MatrixXd pose_jacobian_derivative(const VectorXd& q,
+                                              const VectorXd& q_dot) const override;
 
     DQ raw_fkm(const VectorXd& q) const;
     MatrixXd raw_pose_jacobian(const VectorXd& q, const int& to_link=2) const;
-    MatrixXd raw_pose_jacobian_derivative(const VectorXd& configurations,
-                                          const VectorXd& velocity_configurations, const int& to_link = 2) const;
+    MatrixXd raw_pose_jacobian_derivative(const VectorXd& q,
+                                          const VectorXd& q_dot, const int& to_link = 2) const;
 };
 
 }
diff --git a/src/robot_modeling/DQ_HolonomicBase.cpp b/src/robot_modeling/DQ_HolonomicBase.cpp
index a4f89a1..d301d6e 100644
--- a/src/robot_modeling/DQ_HolonomicBase.cpp
+++ b/src/robot_modeling/DQ_HolonomicBase.cpp
@@ -110,24 +110,20 @@ MatrixXd DQ_HolonomicBase::pose_jacobian(const VectorXd &q) const
 /**
  * @brief returns the Jacobian derivative (J_dot) that satisfies
           vec8(pose_dot_dot) = J_dot * q_dot + J*q_dot_dot, where pose = fkm(), 'pose_dot' is the time
-          derivative of the pose and  joint_configurations is the configuration vector.
+          derivative of the pose and 'q_dot' represents the robot configuration velocities.
           This method does not take into account the base displacement.
- * @param configurations The VectorXd representing the joint configurations.
- * @param velocity_configurations The VectorXd representing the joint velocity configurations.
+ * @param q The VectorXd representing the robot configurations.
+ * @param q_dot The VectorXd representing the robot configuration velocities.
  * @param to_link The ith link which we want to compute the Jacobian derivative.
  * @return a MatrixXd representing the desired Jacobian derivative.
  */
-MatrixXd DQ_HolonomicBase::raw_pose_jacobian_derivative(const VectorXd &configurations, const VectorXd &velocity_configurations, const int &to_link) const
+MatrixXd DQ_HolonomicBase::raw_pose_jacobian_derivative(const VectorXd &q, const VectorXd &q_dot, const int &to_link) const
 {
     if(to_link >= 3 || to_link < 0)
     {
         throw std::runtime_error(std::string("Tried to access link index ") + std::to_string(to_link) + std::string(" which is unnavailable."));
     }
 
-    /// Aliases
-    const VectorXd& q = configurations;
-    const VectorXd& q_dot = velocity_configurations;
-
     const double& x   = q(0);
     const double& y   = q(1);
     const double& phi = q(2);
@@ -165,33 +161,27 @@ MatrixXd DQ_HolonomicBase::raw_pose_jacobian_derivative(const VectorXd &configur
 /**
  * @brief returns the Jacobian derivative 'J_dot' that satisfies
           vec8(pose_dot_dot) = J_dot * q_dot + J*q_dot_dot, where pose = fkm(), 'pose_dot' is the time
-          derivative of the pose and 'q_dot' represents the robot velocity configurations.
- * @param configurations The VectorXd representing the robot configurations.
- * @param velocity_configurations The VectorXd representing the robot velocity configurations.
+          derivative of the pose and 'q_dot' represents the robot configuration velocities.
+ * @param q The VectorXd representing the robot configurations.
+ * @param q_dot The VectorXd representing the robot configuration velocities.
  * @param to_ith_link The 'to_ith_link' link which we want to compute the Jacobian derivative.
  * @return a MatrixXd representing the desired Jacobian derivative.
  */
-MatrixXd DQ_HolonomicBase::pose_jacobian_derivative(const VectorXd &configurations, const VectorXd &velocity_configurations, const int &to_link) const
+MatrixXd DQ_HolonomicBase::pose_jacobian_derivative(const VectorXd &q, const VectorXd &q_dot, const int &to_link) const
 {
-    /// Aliases
-    const VectorXd& q = configurations;
-    const VectorXd& q_dot = velocity_configurations;
     return haminus8(frame_displacement_)*raw_pose_jacobian_derivative(q, q_dot, to_link);
 }
 
 /**
  * @brief returns the Jacobian derivative 'J_dot' that satisfies
           vec8(pose_dot_dot) = J_dot * q_dot + J*q_dot_dot, where pose = fkm(), 'pose_dot' is the time
-          derivative of the pose and 'q_dot' represents the robot velocity configurations.
- * @param configurations The VectorXd representing the robot configurations.
- * @param velocity_configurations The VectorXd representing the robot velocity configurations.
+          derivative of the pose and 'q_dot' represents the robot configuration velocities.
+ * @param q The VectorXd representing the robot configurations.
+ * @param q_dot The VectorXd representing the robot configuration velocities.
  * @return a MatrixXd representing the desired Jacobian derivative.
  */
-MatrixXd DQ_HolonomicBase::pose_jacobian_derivative(const VectorXd &configurations, const VectorXd &velocity_configurations) const
+MatrixXd DQ_HolonomicBase::pose_jacobian_derivative(const VectorXd &q, const VectorXd &q_dot) const
 {
-    /// Aliases
-    const VectorXd& q = configurations;
-    const VectorXd& q_dot = velocity_configurations;
     return pose_jacobian_derivative(q, q_dot, get_dim_configuration_space()-1);
 }
 

From a8dda098ebf11c8c9948355589f3d912c5a9f595 Mon Sep 17 00:00:00 2001
From: Juancho <juanjqogm@gmail.com>
Date: Wed, 16 Nov 2022 17:04:22 +0900
Subject: [PATCH 22/27] [DQ_MobileBase.h] Renamed the variables of
 pose_jacobian_derivative.

---
 include/dqrobotics/robot_modeling/DQ_MobileBase.h | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/include/dqrobotics/robot_modeling/DQ_MobileBase.h b/include/dqrobotics/robot_modeling/DQ_MobileBase.h
index 1e29843..e6a7882 100644
--- a/include/dqrobotics/robot_modeling/DQ_MobileBase.h
+++ b/include/dqrobotics/robot_modeling/DQ_MobileBase.h
@@ -42,7 +42,7 @@ class DQ_MobileBase : public DQ_Kinematics
     //virtual int      get_dim_configuration_space() const = 0;
     //virtual DQ       fkm(const VectorXd& joint_configurations) const = 0;
     //virtual MatrixXd pose_jacobian(const VectorXd& joint_configurations,const int& to_link) const = 0;
-    //virtual MatrixXd pose_jacobian_derivative(const VectorXd& configurations, const VectorXd& velocity_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;
 
     void set_frame_displacement(const DQ& pose);
     DQ   frame_displacement();

From ccadb4ee24a81900ca1a4be843ea36bdf8458b1c Mon Sep 17 00:00:00 2001
From: Juancho <juanjqogm@gmail.com>
Date: Wed, 16 Nov 2022 17:08:01 +0900
Subject: [PATCH 23/27] [DQ_WholeBody.h,.cpp] Renamed the variables of
 pose_jacobian_derivative.

---
 .../dqrobotics/robot_modeling/DQ_WholeBody.h  |  8 ++++----
 src/robot_modeling/DQ_WholeBody.cpp           | 20 ++++++++-----------
 2 files changed, 12 insertions(+), 16 deletions(-)

diff --git a/include/dqrobotics/robot_modeling/DQ_WholeBody.h b/include/dqrobotics/robot_modeling/DQ_WholeBody.h
index d69a218..f0bae17 100644
--- a/include/dqrobotics/robot_modeling/DQ_WholeBody.h
+++ b/include/dqrobotics/robot_modeling/DQ_WholeBody.h
@@ -55,11 +55,11 @@ class DQ_WholeBody : public DQ_Kinematics
     DQ fkm(const VectorXd&, const int& to_chain) const override;
     MatrixXd pose_jacobian(const VectorXd& q, const int& to_ith_chain) const override;
     MatrixXd pose_jacobian(const VectorXd& q) const override;
-    MatrixXd pose_jacobian_derivative(const VectorXd& configurations,
-                                      const VectorXd& velocity_configurations,
+    MatrixXd pose_jacobian_derivative(const VectorXd& q,
+                                      const VectorXd& q_dot,
                                       const int& to_ith_link) const override; //To be implemented.
-    MatrixXd pose_jacobian_derivative (const VectorXd& configurations,
-                                       const VectorXd& velocity_configurations) const override; //To be implemented.
+    MatrixXd pose_jacobian_derivative (const VectorXd& q,
+                                       const VectorXd& q_dot) const override; //To be implemented.
 };
 
 }
diff --git a/src/robot_modeling/DQ_WholeBody.cpp b/src/robot_modeling/DQ_WholeBody.cpp
index 9e53235..489bef4 100644
--- a/src/robot_modeling/DQ_WholeBody.cpp
+++ b/src/robot_modeling/DQ_WholeBody.cpp
@@ -175,13 +175,13 @@ void DQ_WholeBody::set_effector(const DQ &effector)
 /**
  * @brief returns the Jacobian derivative 'J_dot' that satisfies
           vec8(pose_dot_dot) = J_dot * q_dot + J*q_dot_dot, where pose = fkm(), 'pose_dot' is the time
-          derivative of the pose and 'q_dot' represents the robot velocity configurations.
- * @param configurations The VectorXd representing the robot configurations.
- * @param velocity_configurations The VectorXd representing the robot velocity configurations.
+          derivative of the pose and 'q_dot' represents the robot configuration velocities.
+ * @param q The VectorXd representing the robot configurations.
+ * @param q_dot The VectorXd representing the robot configuration velocities.
  * @param to_ith_link The 'to_ith_link' link which we want to compute the Jacobian derivative.
  * @return a MatrixXd representing the desired Jacobian derivative.
  */
-MatrixXd DQ_WholeBody::pose_jacobian_derivative(const VectorXd &configurations, const VectorXd &velocity_configurations, const int &to_ith_link) const
+MatrixXd DQ_WholeBody::pose_jacobian_derivative(const VectorXd &q, const VectorXd &q_dot, const int &to_ith_link) const
 {
     throw std::runtime_error(std::string("pose_jacobian_derivative is not implemented yet."));
     return MatrixXd::Zero(1,1);
@@ -191,17 +191,13 @@ MatrixXd DQ_WholeBody::pose_jacobian_derivative(const VectorXd &configurations,
 /**
  * @brief returns the Jacobian derivative 'J_dot' that satisfies
           vec8(pose_dot_dot) = J_dot * q_dot + J*q_dot_dot, where pose = fkm(), 'pose_dot' is the time
-          derivative of the pose and 'q_dot' represents the robot velocity configurations.
- * @param configurations The VectorXd representing the robot configurations.
- * @param velocity_configurations The VectorXd representing the robot velocity configurations.
+          derivative of the pose and 'q_dot' represents the robot configuration velocities.
+ * @param q The VectorXd representing the robot configurations.
+ * @param q_dot The VectorXd representing the robot configuration velocities.
  * @return a MatrixXd representing the desired Jacobian derivative.
  */
-MatrixXd DQ_WholeBody::pose_jacobian_derivative(const VectorXd &configurations, const VectorXd &velocity_configurations) const
+MatrixXd DQ_WholeBody::pose_jacobian_derivative(const VectorXd &q, const VectorXd &q_dot) const
 {
-    /// Aliases
-    const VectorXd& q = configurations;
-    const VectorXd& q_dot = velocity_configurations;
-
     return pose_jacobian_derivative(q,q_dot,  get_dim_configuration_space()-1);
 }
 

From b45976e60250b614eba43f25b97f55a66f664ff3 Mon Sep 17 00:00:00 2001
From: Juancho <juanjqogm@gmail.com>
Date: Wed, 16 Nov 2022 17:12:22 +0900
Subject: [PATCH 24/27] [DQ_SerialWholeBody.h,.cpp] Renamed the variables of
 pose_jacobian_derivative.

---
 .../robot_modeling/DQ_SerialWholeBody.h       | 12 ++++-----
 src/robot_modeling/DQ_SerialWholeBody.cpp     | 25 +++++++------------
 2 files changed, 15 insertions(+), 22 deletions(-)

diff --git a/include/dqrobotics/robot_modeling/DQ_SerialWholeBody.h b/include/dqrobotics/robot_modeling/DQ_SerialWholeBody.h
index ca69729..9721ce9 100644
--- a/include/dqrobotics/robot_modeling/DQ_SerialWholeBody.h
+++ b/include/dqrobotics/robot_modeling/DQ_SerialWholeBody.h
@@ -58,8 +58,8 @@ class DQ_SerialWholeBody : public DQ_Kinematics
     DQ_SerialManipulatorDH get_chain_as_serial_manipulator_dh(const int& to_ith_chain) const;
     DQ_HolonomicBase get_chain_as_holonomic_base(const int& to_ith_chain) const;
     MatrixXd raw_pose_jacobian_by_chain(const VectorXd& q, const int& to_ith_chain, const int& to_jth_link) const;
-    MatrixXd raw_pose_jacobian_derivative_by_chain(const VectorXd& configurations,
-                                                   const VectorXd& velocity_configurations,
+    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.
 
@@ -68,11 +68,11 @@ class DQ_SerialWholeBody : public DQ_Kinematics
     DQ fkm(const VectorXd&, const int& to_ith_link) const override;
     MatrixXd pose_jacobian(const VectorXd& q, const int& to_ith_link) const override;
     MatrixXd pose_jacobian(const VectorXd& q) const override;
-    MatrixXd pose_jacobian_derivative(const VectorXd& configurations,
-                                      const VectorXd& velocity_configurations,
+    MatrixXd pose_jacobian_derivative(const VectorXd& q,
+                                      const VectorXd& q_dot,
                                       const int& to_ith_link) const override; //To be implemented.
-    MatrixXd pose_jacobian_derivative (const VectorXd& configurations,
-                                       const VectorXd& velocity_configurations) const override; //To be implemented.
+    MatrixXd pose_jacobian_derivative (const VectorXd& q,
+                                       const VectorXd& q_dot) const override; //To be implemented.
 };
 
 }
diff --git a/src/robot_modeling/DQ_SerialWholeBody.cpp b/src/robot_modeling/DQ_SerialWholeBody.cpp
index 74707c7..55c4a4b 100644
--- a/src/robot_modeling/DQ_SerialWholeBody.cpp
+++ b/src/robot_modeling/DQ_SerialWholeBody.cpp
@@ -251,7 +251,7 @@ MatrixXd DQ_SerialWholeBody::pose_jacobian(const VectorXd &q) const
 }
 
 //To be implemented.
-MatrixXd DQ_SerialWholeBody::raw_pose_jacobian_derivative_by_chain(const VectorXd &configurations, const VectorXd &velocity_configurations, const int &to_ith_chain, const int &to_jth_link) const
+MatrixXd DQ_SerialWholeBody::raw_pose_jacobian_derivative_by_chain(const VectorXd &q, const VectorXd &q_dot, const int &to_ith_chain, const int &to_jth_link) const
 {
     throw std::runtime_error(std::string("pose_jacobian_derivative_by_chain is not implemented yet."));
     return MatrixXd::Zero(1,1);
@@ -260,21 +260,17 @@ MatrixXd DQ_SerialWholeBody::raw_pose_jacobian_derivative_by_chain(const VectorX
 /**
  * @brief returns the Jacobian derivative 'J_dot' that satisfies
           vec8(pose_dot_dot) = J_dot * q_dot + J*q_dot_dot, where pose = fkm(), 'pose_dot' is the time
-          derivative of the pose and 'q_dot' represents the robot velocity configurations.
- * @param configurations The VectorXd representing the robot configurations.
- * @param velocity_configurations The VectorXd representing the robot velocity configurations.
+          derivative of the pose and 'q_dot' represents the robot configuration velocities.
+ * @param q The VectorXd representing the robot configurations.
+ * @param q_dot The VectorXd representing the robot configuration velocities.
  * @param to_ith_link The 'to_ith_link' link which we want to compute the Jacobian derivative.
  * @return a MatrixXd representing the desired Jacobian derivative.
  */
-MatrixXd DQ_SerialWholeBody::pose_jacobian_derivative(const VectorXd &configurations, const VectorXd &velocity_configurations, const int &to_ith_link) const
+MatrixXd DQ_SerialWholeBody::pose_jacobian_derivative(const VectorXd &q, const VectorXd &q_dot, const int &to_ith_link) const
 {
     int to_ith_chain;
     int to_jth_link;
 
-    /// Aliases
-    const VectorXd& q = configurations;
-    const VectorXd& q_dot = velocity_configurations;
-
     std::tie(to_ith_chain,to_jth_link) = get_chain_and_link_from_index(to_ith_link);
     return raw_pose_jacobian_derivative_by_chain(q, q_dot, to_ith_chain,to_ith_link);
 }
@@ -283,16 +279,13 @@ MatrixXd DQ_SerialWholeBody::pose_jacobian_derivative(const VectorXd &configurat
 /**
  * @brief returns the Jacobian derivative 'J_dot' that satisfies
           vec8(pose_dot_dot) = J_dot * q_dot + J*q_dot_dot, where pose = fkm(), 'pose_dot' is the time
-          derivative of the pose and 'q_dot' represents the robot velocity configurations.
- * @param configurations The VectorXd representing the robot configurations.
- * @param velocity_configurations The VectorXd representing the robot velocity configurations.
+          derivative of the pose and 'q_dot' represents the robot configuration velocities.
+ * @param q The VectorXd representing the robot configurations.
+ * @param q_dot The VectorXd representing the robot configuration velocities.
  * @return a MatrixXd representing the desired Jacobian derivative.
  */
-MatrixXd DQ_SerialWholeBody::pose_jacobian_derivative(const VectorXd &configurations, const VectorXd &velocity_configurations) const
+MatrixXd DQ_SerialWholeBody::pose_jacobian_derivative(const VectorXd &q, const VectorXd &q_dot) const
 {
-    /// Aliases
-    const VectorXd& q = configurations;
-    const VectorXd& q_dot = velocity_configurations;
     return pose_jacobian_derivative(q,q_dot,  get_dim_configuration_space()-1);
 }
 

From eb0a6609d368fead61a5948023868552a0ce5962 Mon Sep 17 00:00:00 2001
From: Juancho <juanjqogm@gmail.com>
Date: Wed, 16 Nov 2022 17:21:47 +0900
Subject: [PATCH 25/27] [DQ_SerialManipulator.h,.cpp] Renamed the variables of
 pose_jacobian_derivative.

---
 .../robot_modeling/DQ_SerialManipulator.h     |  8 ++--
 src/robot_modeling/DQ_SerialManipulator.cpp   | 43 +++++++++----------
 2 files changed, 25 insertions(+), 26 deletions(-)

diff --git a/include/dqrobotics/robot_modeling/DQ_SerialManipulator.h b/include/dqrobotics/robot_modeling/DQ_SerialManipulator.h
index 525c6e1..c70e254 100644
--- a/include/dqrobotics/robot_modeling/DQ_SerialManipulator.h
+++ b/include/dqrobotics/robot_modeling/DQ_SerialManipulator.h
@@ -50,12 +50,12 @@ class DQ_SerialManipulator: public DQ_Kinematics
 
     //Virtual
     virtual MatrixXd raw_pose_jacobian(const VectorXd& q_vec) const;
-    virtual MatrixXd raw_pose_jacobian_derivative(const VectorXd& q_vec, const VectorXd& q_vec_dot) 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_vec, const VectorXd& q_vec_dot, 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;
 
     //Overrides from DQ_Kinematics
@@ -66,8 +66,8 @@ class DQ_SerialManipulator: public 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_vec, const VectorXd& q_vec_dot, const int& to_ith_link) const override; //Override from DQ_Kinematics
-    virtual MatrixXd pose_jacobian_derivative(const VectorXd& q_vec, const VectorXd& q_vec_dot) 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
 
 };
 
diff --git a/src/robot_modeling/DQ_SerialManipulator.cpp b/src/robot_modeling/DQ_SerialManipulator.cpp
index 3c1f388..68f1ebb 100644
--- a/src/robot_modeling/DQ_SerialManipulator.cpp
+++ b/src/robot_modeling/DQ_SerialManipulator.cpp
@@ -161,35 +161,34 @@ MatrixXd DQ_SerialManipulator::pose_jacobian(const VectorXd &q_vec) const
 /**
  * @brief This method returns the time derivative of the pose Jacobian.
  *        The base displacement and the effector are not taken into account.
- * @param q_vec. Vector of joint values.
- * @param q_vec_dot. Vector of joint velocity values.
-                  will be calculated.
- * @returns The first to_ith_link columns of the pose_jacobian_derivative.
+ * @param q. VectorXd representing the robot joint configuration.
+ * @param q_dot. VectorXd representing the robot joint velocities.
+ * @returns a MatrixXd representing the desired Jacobian derivative.
  *
  */
-MatrixXd DQ_SerialManipulator::raw_pose_jacobian_derivative(const VectorXd &q_vec, const VectorXd &q_vec_dot) const
+MatrixXd DQ_SerialManipulator::raw_pose_jacobian_derivative(const VectorXd &q, const VectorXd &q_dot) const
 {
-    _check_q_vec(q_vec);
-    _check_q_vec(q_vec_dot);
-    return raw_pose_jacobian_derivative(q_vec, q_vec_dot, get_dim_configuration_space()-1);
+    _check_q_vec(q);
+    _check_q_vec(q_dot);
+    return raw_pose_jacobian_derivative(q, q_dot, get_dim_configuration_space()-1);
 }
 
 
 /**
  * @brief This method returns the first to_ith_link columns of the time derivative of the pose Jacobian.
- * @param q_vec. Vector of joint values.
- * @param q_vec_dot. Vector of joint velocity values.
+ * @param q. VectorXd representing the robot joint configuration.
+ * @param q_dot. VectorXd representing the robot joint velocities.
  * @param to_ith_link. The index to a link. This defines until which link the pose_jacobian_derivative
  *                     will be calculated.
- * @returns The first to_ith_link columns of the pose_jacobian_derivative.
+ * @returns a MatrixXd representing the first to_ith_link columns of the desired Jacobian derivative.
  *
  */
-MatrixXd  DQ_SerialManipulator::pose_jacobian_derivative(const VectorXd &q_vec, const VectorXd &q_vec_dot, const int &to_ith_link) const
+MatrixXd  DQ_SerialManipulator::pose_jacobian_derivative(const VectorXd &q, const VectorXd &q_dot, const int &to_ith_link) const
 {
-    _check_q_vec(q_vec);
-    _check_q_vec(q_vec_dot);
+    _check_q_vec(q);
+    _check_q_vec(q_dot);
     _check_to_ith_link(to_ith_link);
-    MatrixXd J_dot = raw_pose_jacobian_derivative(q_vec, q_vec_dot, to_ith_link);
+    MatrixXd J_dot = raw_pose_jacobian_derivative(q, q_dot, to_ith_link);
 
     if(to_ith_link==this->get_dim_configuration_space()-1)
     {
@@ -204,16 +203,16 @@ MatrixXd  DQ_SerialManipulator::pose_jacobian_derivative(const VectorXd &q_vec,
 
 /**
  * @brief This method returns time derivative of the pose Jacobian.
- * @param q_vec. Vector of joint values.
- * @param q_vec_dot. Vector of joint velocity values.
- * @returns The desired pose Jacobian derivative.
+ * @param q. VectorXd representing the robot joint configuration.
+ * @param q_dot. VectorXd representing the robot joint velocities.
+ * @returns a MatrixXd representing the desired Jacobian derivative.
  *
  */
-MatrixXd DQ_SerialManipulator::pose_jacobian_derivative(const VectorXd &q_vec, const VectorXd &q_vec_dot) const
+MatrixXd DQ_SerialManipulator::pose_jacobian_derivative(const VectorXd &q, const VectorXd &q_dot) const
 {
-    _check_q_vec(q_vec);
-    _check_q_vec(q_vec_dot);
-    return this->DQ_Kinematics::pose_jacobian_derivative(q_vec, q_vec_dot);
+    _check_q_vec(q);
+    _check_q_vec(q_dot);
+    return this->DQ_Kinematics::pose_jacobian_derivative(q, q_dot);
 }
 
 }//namespace DQ_robotics

From 08bb85f9109a3862c42e3ee1b57553390fbe2219 Mon Sep 17 00:00:00 2001
From: Juancho <juanjqogm@gmail.com>
Date: Wed, 16 Nov 2022 17:25:25 +0900
Subject: [PATCH 26/27] [DQ_SerialManipulatorDH.h,.cpp] Renamed the variables
 of pose_jacobian_derivative.

---
 .../robot_modeling/DQ_SerialManipulatorDH.h   |  2 +-
 src/robot_modeling/DQ_SerialManipulatorDH.cpp | 23 +++++++++----------
 2 files changed, 12 insertions(+), 13 deletions(-)

diff --git a/include/dqrobotics/robot_modeling/DQ_SerialManipulatorDH.h b/include/dqrobotics/robot_modeling/DQ_SerialManipulatorDH.h
index c028b57..7e50fad 100644
--- a/include/dqrobotics/robot_modeling/DQ_SerialManipulatorDH.h
+++ b/include/dqrobotics/robot_modeling/DQ_SerialManipulatorDH.h
@@ -53,7 +53,7 @@ class DQ_SerialManipulatorDH: public DQ_SerialManipulator
     using DQ_SerialManipulator::raw_fkm;
 
     MatrixXd raw_pose_jacobian(const VectorXd& q_vec, const int& to_ith_link) const override;
-    MatrixXd raw_pose_jacobian_derivative(const VectorXd& q_vec, const VectorXd& q_vec_dot, const int& to_ith_link) const override;
+    MatrixXd raw_pose_jacobian_derivative(const VectorXd& q, const VectorXd& q_dot, const int& to_ith_link) const override;
     DQ raw_fkm(const VectorXd &q_vec, const int &to_ith_link) const override;
 };
 
diff --git a/src/robot_modeling/DQ_SerialManipulatorDH.cpp b/src/robot_modeling/DQ_SerialManipulatorDH.cpp
index 88eeeff..920b28b 100644
--- a/src/robot_modeling/DQ_SerialManipulatorDH.cpp
+++ b/src/robot_modeling/DQ_SerialManipulatorDH.cpp
@@ -248,24 +248,23 @@ MatrixXd DQ_SerialManipulatorDH::raw_pose_jacobian(const VectorXd &q_vec, const
 /**
  * @brief This method returns the first to_ith_link columns of the time derivative of the pose Jacobian.
  *        The base displacement and the effector are not taken into account.
- * @param q_vec. Vector of joint values.
- * @param q_vec_dot. Vector of joint velocity values.
-                  will be calculated.
+ * @param q. VectorXd representing the robot joint configuration.
+ * @param q_dot. VectorXd representing the robot joint velocities.
  * @param to_ith_link. The index to a link. This defines until which link the pose_jacobian_derivative
  *                     will be calculated.
- * @returns The first to_ith_link columns of the pose_jacobian_derivative.
+ * @returns a MatrixXd representing the first to_ith_link columns of the desired Jacobian derivative.
  *
  */
-MatrixXd DQ_SerialManipulatorDH::raw_pose_jacobian_derivative(const VectorXd &q_vec, const VectorXd &q_vec_dot, const int &to_ith_link) const
+MatrixXd DQ_SerialManipulatorDH::raw_pose_jacobian_derivative(const VectorXd &q, const VectorXd &q_dot, const int &to_ith_link) const
 {
-    _check_q_vec(q_vec);
-    _check_q_vec(q_vec_dot);
+    _check_q_vec(q);
+    _check_q_vec(q_dot);
     _check_to_ith_link(to_ith_link);
 
     int n = to_ith_link+1;
-    DQ x_effector = raw_fkm(q_vec,to_ith_link);
-    MatrixXd J    = raw_pose_jacobian(q_vec,to_ith_link);
-    VectorXd vec_x_effector_dot = J*q_vec_dot.head(n);
+    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);
     MatrixXd J_dot = MatrixXd::Zero(8,n);
     int jth=0;
@@ -282,11 +281,11 @@ MatrixXd DQ_SerialManipulatorDH::raw_pose_jacobian_derivative(const VectorXd &q_
         }
         else
         {
-            vec_zdot = 0.5*(haminus8(w*conj(x)) + hamiplus8(x*w)*C8())*raw_pose_jacobian(q_vec,i-1)*q_vec_dot.head(i);
+            vec_zdot = 0.5*(haminus8(w*conj(x)) + hamiplus8(x*w)*C8())*raw_pose_jacobian(q,i-1)*q_dot.head(i);
         }
 
         J_dot.col(jth) = haminus8(x_effector)*vec_zdot + hamiplus8(z)*vec_x_effector_dot;
-        x = x*_dh2dq(q_vec(jth),i);
+        x = x*_dh2dq(q(jth),i);
         jth = jth+1;
     }
 

From 3104db476e74229c6742cbf1742b2fd13163a5a5 Mon Sep 17 00:00:00 2001
From: Juancho <juanjqogm@gmail.com>
Date: Wed, 16 Nov 2022 17:28:58 +0900
Subject: [PATCH 27/27] [DQ_SerialManipulatorMDH.h,.cpp] Renamed the variables
 of pose_jacobian_derivative.

---
 .../robot_modeling/DQ_SerialManipulatorMDH.h  |  2 +-
 .../DQ_SerialManipulatorMDH.cpp               | 23 +++++++++----------
 2 files changed, 12 insertions(+), 13 deletions(-)

diff --git a/include/dqrobotics/robot_modeling/DQ_SerialManipulatorMDH.h b/include/dqrobotics/robot_modeling/DQ_SerialManipulatorMDH.h
index 3f3f66b..b9a1c45 100644
--- a/include/dqrobotics/robot_modeling/DQ_SerialManipulatorMDH.h
+++ b/include/dqrobotics/robot_modeling/DQ_SerialManipulatorMDH.h
@@ -52,7 +52,7 @@ class DQ_SerialManipulatorMDH: public DQ_SerialManipulator
     using DQ_SerialManipulator::raw_fkm;
 
     MatrixXd raw_pose_jacobian(const VectorXd& q_vec, const int& to_ith_link) const override;
-    MatrixXd raw_pose_jacobian_derivative(const VectorXd& q_vec, const VectorXd& q_vec_dot, const int& to_ith_link) const override;
+    MatrixXd raw_pose_jacobian_derivative(const VectorXd& q, const VectorXd& q_dot, const int& to_ith_link) const override;
     DQ raw_fkm(const VectorXd &q_vec, const int &to_ith_link) const override;
 };
 
diff --git a/src/robot_modeling/DQ_SerialManipulatorMDH.cpp b/src/robot_modeling/DQ_SerialManipulatorMDH.cpp
index ede0284..83f1ed9 100644
--- a/src/robot_modeling/DQ_SerialManipulatorMDH.cpp
+++ b/src/robot_modeling/DQ_SerialManipulatorMDH.cpp
@@ -200,24 +200,23 @@ VectorXd DQ_SerialManipulatorMDH::get_types() const
 /**
  * @brief This method returns the first to_ith_link columns of the time derivative of the pose Jacobian.
  *        The base displacement and the effector are not taken into account.
- * @param q_vec. Vector of joint values.
- * @param q_vec_dot. Vector of joint velocity values.
-                  will be calculated.
+ * @param q. VectorXd representing the robot joint configuration.
+ * @param q_dot. VectorXd representing the robot joint velocities.
  * @param to_ith_link. The index to a link. This defines until which link the pose_jacobian_derivative
  *                     will be calculated.
- * @returns The first to_ith_link columns of the pose_jacobian_derivative.
+ * @returns a MatrixXd representing the first to_ith_link columns of the desired Jacobian derivative.
  *
  */
-MatrixXd DQ_SerialManipulatorMDH::raw_pose_jacobian_derivative(const VectorXd &q_vec, const VectorXd &q_vec_dot, const int &to_ith_link) const
+MatrixXd DQ_SerialManipulatorMDH::raw_pose_jacobian_derivative(const VectorXd &q, const VectorXd &q_dot, const int &to_ith_link) const
 {
-    _check_q_vec(q_vec);
-    _check_q_vec(q_vec_dot);
+    _check_q_vec(q);
+    _check_q_vec(q_dot);
     _check_to_ith_link(to_ith_link);
 
     int n = to_ith_link+1;
-    DQ x_effector = raw_fkm(q_vec,to_ith_link);
-    MatrixXd J    = raw_pose_jacobian(q_vec,to_ith_link);
-    VectorXd vec_x_effector_dot = J*q_vec_dot.head(n); //(to_ith_link);
+    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); //(to_ith_link);
 
     DQ x = DQ(1);
     MatrixXd J_dot = MatrixXd::Zero(8,n);
@@ -235,11 +234,11 @@ MatrixXd DQ_SerialManipulatorMDH::raw_pose_jacobian_derivative(const VectorXd &q
         }
         else
         {
-            vec_zdot = 0.5*(haminus8(w*conj(x)) + hamiplus8(x*w)*C8())*raw_pose_jacobian(q_vec,i-1)*q_vec_dot.head(i);
+            vec_zdot = 0.5*(haminus8(w*conj(x)) + hamiplus8(x*w)*C8())*raw_pose_jacobian(q,i-1)*q_dot.head(i);
         }
 
         J_dot.col(jth) = haminus8(x_effector)*vec_zdot + hamiplus8(z)*vec_x_effector_dot;
-        x = x*_mdh2dq(q_vec(jth),i);
+        x = x*_mdh2dq(q(jth),i);
         jth = jth+1;
     }