This PR adds the pose_jacobian_derivative method in the DQ_Kinematics class.

[juanjqo]

@dqrobotics/developers

Hi @mmmarinho,

This PR adds the pose_jacobian_derivative method in the DQ_Kinematics class. I included an example here.

DQ_Kinematics has now the following two methods:

    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_derivative(const VectorXd& joint_configurations,
                                              const VectorXd& joint_velocity_configurations) const;

DQ_SerialManipulator has now the following two methods:

    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

DQ_SerialManipulatorDH and DQ_SerialManipulatorMDH have now the following methods:

    using DQ_SerialManipulator::raw_pose_jacobian_derivative;
    MatrixXd raw_pose_jacobian_derivative(const VectorXd& q_vec, const VectorXd& q_vec_dot, const int& to_ith_link) const override; 

DQ_DifferentialDriveRobot() has now the following method:

    MatrixXd pose_jacobian_derivative (const VectorXd& q,
                                       const VectorXd& q_dot) const override;

DQ_HolonomicBase() has now the following methods:

 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

 MatrixXd raw_pose_jacobian_derivative(const VectorXd& q,
                                          const VectorXd& q_dot, const int& to_link = 2) const;

Current limitations:

The classes DQ_Serial WholeBody() and DQ_WholeBody do have not the pose jacobian derivative method available for the user. If a user tries to use it the method raises an exception.

Example:

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

Proposed Modifications (UML Diagram)

The new methods (pose_jacobian_derivative) are highlighted in blue

[bvadorno]

Hi @juanjqo,

Thank you for the updated class diagram. Since DQ_Kinematics is an abstract class used with robots other than manipulators, it doesn't make much sense to use the variables **joint**_configurations and **joint**_velocity_configurations because some robots don't have joints.

I suggest using the variables either robot_configurations or simply configurations (and analogously for the velocities) in DQ_Kinematics and all subclasses of robots that don't have joints. I don't remember if we expose those variables by means of get and set methods. If we do, then we'd need to create aliases in DQ_SerialManipulator to maintain backward compatibility.

Best wishes,
Bruno

[juanjqo]

@dqrobotics/developers

Hi Bruno,

Thanks for your feedback. I implemented your suggestion. I renamed the variables in this way:

(For all classes except DQ_SerialManipulator)

pose_jacobian_derivative(configurations, velocity_configurations, to_ith_link);
pose_jacobian_derivative(configurations, velocity_configurations);

Best regards,

Juancho

Update (Juancho)

I only modified the variables of the method pose_jacobian_derivative. Other methods as fkm and pose_jacobian will require similar modifications to keep consistency. However, these modifications will be addressed in a different PR.

[mmmarinho]

@juanjqo Could we change from
velocity_configurations
to
configuration_velocities

or just stick to the q and q_dot we were using before?

Now it seems that all the subclasses have different parameter names for some reason.

I know that some are remnants of older versions of dqrobotics, but we can take this chance to unify their naming. Whatever we do, if we keep the naming consistent, it will be much easier to refactor when needed.

[juanjqo]

@mmmarinho Sure! I personally like q and q_dot more than any other. I used the same names currently used by fkm(), pose_jacobian(), etc.

Update

@mmmarinho I modified the variables as you suggested for all classes (only for pose_jacobian_derivative). Furthermore, I updated the documentation.

Example:

/**
 * @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)

Best regards,

Juancho

[mmmarinho]

@juanjqo I’ll accept this one so the process on the Python version can also start.

Regardless of it being incorporated on the master branch, we (@bvadorno @juanjqo @mmmarinho ) can keep discussing and if necessary further refine this initial proposal.

[juanjqo]

@dqrobotics/developers

@mmmarinho sure!

I'm working on the Python version.