This PR adds the pose_jacobian_derivative method in the DQ_Kinematics class.

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;
 };
 
 }

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;
 };
 
 }

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& 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& q,
+                                              const VectorXd& q_dot) const;
     virtual int get_dim_configuration_space() const;
 
     //Static methods

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& q, const VectorXd& q_dot, const int& to_link) const = 0;
 
     void set_frame_displacement(const DQ& pose);
     DQ   frame_displacement();

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, const VectorXd& q_dot) const;
     virtual DQ raw_fkm(const VectorXd& q_vec) const;
 
     //Pure virtual
     virtual MatrixXd raw_pose_jacobian(const VectorXd& q_vec, const int& to_ith_link) const = 0;
+    virtual MatrixXd raw_pose_jacobian_derivative(const VectorXd& q, const VectorXd& q_dot, const int& to_ith_link) const = 0;
     virtual DQ raw_fkm(const VectorXd& q_vec, const int& to_ith_link) const = 0;
 
     //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, 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
+
 };
 
 }

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, 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;
 };
 

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, 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;
 };
 

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.
 };
 
 }

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.
 };
 
 }

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,40 @@ 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 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 &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}.");
+
+    /// 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));
+    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 velocities.
+ * @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);
+}
+
 }

src/robot_modeling/DQ_HolonomicBase.cpp

@@ -107,4 +107,82 @@ 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 'q_dot' represents the robot configuration velocities.
+          This method does not take into account the base displacement.
+ * @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 &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 '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 &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 '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 &q, const VectorXd &q_dot) const
+{
+    return pose_jacobian_derivative(q, q_dot, get_dim_configuration_space()-1);
+}
+
 }

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 '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 &q, const VectorXd &q_dot) const
+{
+    return pose_jacobian_derivative(q, q_dot, get_dim_configuration_space()-1);
+}
+
 /**
  * @brief Returns the dimensions of the configuration space.
  * @return An int presenting the dimension of the configuration space.