This PR adds the class DQ_FreeFlyingRobot()

CMakeLists.txt

@@ -57,6 +57,7 @@ ADD_LIBRARY(dqrobotics SHARED
     src/robot_modeling/DQ_DifferentialDriveRobot.cpp
     src/robot_modeling/DQ_WholeBody.cpp
     src/robot_modeling/DQ_SerialWholeBody.cpp
+    src/robot_modeling/DQ_FreeFlyingRobot.cpp
 
     src/robot_control/DQ_KinematicController.cpp
     src/robot_control/DQ_PseudoinverseController.cpp
@@ -111,6 +112,7 @@ INSTALL(FILES
     include/dqrobotics/robot_modeling/DQ_DifferentialDriveRobot.h
     include/dqrobotics/robot_modeling/DQ_WholeBody.h
     include/dqrobotics/robot_modeling/DQ_SerialWholeBody.h
+    include/dqrobotics/robot_modeling/DQ_FreeFlyingRobot.h
     DESTINATION "include/dqrobotics/robot_modeling")
 
 # robot_control headers
@@ -181,6 +183,7 @@ INSTALL(FILES
     src/robot_modeling/DQ_DifferentialDriveRobot.cpp
     src/robot_modeling/DQ_WholeBody.cpp
     src/robot_modeling/DQ_SerialWholeBody.cpp
+    src/robot_modeling/DQ_FreeFlyingRobot.cpp
     DESTINATION "src/dqrobotics/robot_modeling")
 
 # robots folder

include/dqrobotics/robot_modeling/DQ_FreeFlyingRobot.h

@@ -0,0 +1,62 @@
+/**
+(C) Copyright 2023 DQ Robotics Developers
+
+This file is part of DQ Robotics.
+
+    DQ Robotics is free software: you can redistribute it and/or modify
+    it under the terms of the GNU Lesser General Public License as published by
+    the Free Software Foundation, either version 3 of the License, or
+    (at your option) any later version.
+
+    DQ Robotics is distributed in the hope that it will be useful,
+    but WITHOUT ANY WARRANTY; without even the implied warranty of
+    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+    GNU Lesser General Public License for more details.
+
+    You should have received a copy of the GNU Lesser General Public License
+    along with DQ Robotics.  If not, see <http://www.gnu.org/licenses/>.
+
+Contributors:
+
+    1. Bruno Vilhena Adorno ([email protected])
+       Responsible for the original implementation in file DQ_FreeFlyingRobot.m
+       https://github.com/dqrobotics/matlab/pull/66/commits
+
+    2. Juan Jose Quiroz Omana   ([email protected])
+       Created this file.
+*/
+
+#pragma once
+#include <dqrobotics/robot_modeling/DQ_Kinematics.h>
+
+namespace DQ_robotics
+{
+
+class DQ_FreeFlyingRobot: public DQ_Kinematics
+{
+protected:
+
+    void _invalid_signature(const std::string& msg) const;
+
+public:
+    DQ_FreeFlyingRobot();
+
+    //Virtual method overloads (DQ_Kinematics)
+    virtual DQ fkm                (const VectorXd&) const override;
+    virtual DQ fkm                (const VectorXd&, const int&) const override;
+    virtual MatrixXd pose_jacobian(const VectorXd&, const int&) const override;
+    virtual MatrixXd pose_jacobian_derivative(const VectorXd& q,
+                                              const VectorXd& q_dot,
+                                              const int& to_ith_link) const override;
+
+
+
+
+    DQ fkm(const DQ& pose) const;
+    MatrixXd pose_jacobian(const DQ& pose) const;
+    MatrixXd pose_jacobian_derivative(const DQ& pose_derivative) const;
+
+};
+
+}
+

src/robot_modeling/DQ_FreeFlyingRobot.cpp

@@ -0,0 +1,143 @@
+/**
+(C) Copyright 2023 DQ Robotics Developers
+
+This file is part of DQ Robotics.
+
+    DQ Robotics is free software: you can redistribute it and/or modify
+    it under the terms of the GNU Lesser General Public License as published by
+    the Free Software Foundation, either version 3 of the License, or
+    (at your option) any later version.
+
+    DQ Robotics is distributed in the hope that it will be useful,
+    but WITHOUT ANY WARRANTY; without even the implied warranty of
+    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+    GNU Lesser General Public License for more details.
+
+    You should have received a copy of the GNU Lesser General Public License
+    along with DQ Robotics.  If not, see <http://www.gnu.org/licenses/>.
+
+Contributors:
+
+    1. Bruno Vilhena Adorno ([email protected])
+       Responsible for the original implementation in file DQ_FreeFlyingRobot.m
+       https://github.com/dqrobotics/matlab/pull/66/commits
+
+    2. Juan Jose Quiroz Omana   ([email protected])
+       Created this file.
+*/
+
+#include <dqrobotics/robot_modeling/DQ_FreeFlyingRobot.h>
+
+namespace DQ_robotics
+{
+
+/**
+ * @brief Constructor of the DQ_FreeFlyingRobot() class.
+ */
+DQ_FreeFlyingRobot::DQ_FreeFlyingRobot()
+{
+    dim_configuration_space_ = 8;
+}
+
+
+/**
+ * @brief Internal method to handle invalid signatures. This method
+ *        throws an exception with the following message:
+ *
+ *        "This signature is not supported in this class. "
+ *        msg + ", where pose is a unit dual quaternion, which "
+ *        "represents the free-flying robot configuration. "
+ *
+ * @param msg Message to provide the valid signature. Ex:
+ *
+ *         auto msg = std::string("Use fkm(pose)");
+ *         _invalid_signature(msg);
+ */
+void DQ_FreeFlyingRobot::_invalid_signature(const std::string &msg) const
+{
+    throw std::runtime_error(std::string("This signature is not supported in this class. ") +
+                             msg + std::string(", where pose is a unit dual quaternion, which ")+
+                             std::string("represents the free-flying robot configuration. "));
+}
+
+
+DQ DQ_FreeFlyingRobot::fkm(const VectorXd&) const
+{
+    _invalid_signature(std::string("Use fkm(pose)"));
+    return DQ(1);
+}
+
+
+DQ DQ_FreeFlyingRobot::fkm(const VectorXd&, const int&) const
+{
+    _invalid_signature(std::string("Use fkm(pose)"));
+    return DQ(1);
+}
+
+MatrixXd DQ_FreeFlyingRobot::pose_jacobian(const VectorXd&, const int&) const
+{
+    _invalid_signature(std::string("Use pose_jacobian(pose)"));
+    return Eigen::MatrixXd::Zero(2,2);
+}
+
+MatrixXd DQ_FreeFlyingRobot::pose_jacobian_derivative(const VectorXd&, const VectorXd&, const int&) const
+{
+    _invalid_signature(std::string("Use pose_jacobian_derivative(pose_derivative). "
+                                   "pose_derivative denotes the time derivative of pose"));
+    return Eigen::MatrixXd::Zero(2,2);
+}
+
+
+/**
+ * @brief This method returns the free-flying robot pose.
+ * @param pose The unit dual quaternion that represents
+ *             the free-flying robot configuration.
+ * @return pose The free-flying robot pose.
+ */
+DQ DQ_FreeFlyingRobot::fkm(const DQ& pose) const
+{
+    return pose;
+}
+
+/**
+ * @brief This method returns the pose Jacobian J that satisfies
+ *
+ *        x_dot = J*twist,
+ *
+ *        where x_dot is the time derivative of the unit dual quaternion
+ *        that represents the free-flying robot pose, and twist = vec8(csi),
+ *        where csi satisfies the dual quaternion propagation equation
+ *        xdot = (1/2)*csi*x.
+ *
+ * @param pose The unit dual quaternion that represents
+ *             the free-flying robot configuration.
+ * @return The desired pose Jacobian.
+ */
+MatrixXd DQ_FreeFlyingRobot::pose_jacobian(const DQ &pose) const
+{
+    return haminus8(0.5*pose);
+}
+
+
+/**
+ * @brief This method returns the pose Jacobian derivative of J, where
+ *        J is the pose Jacobian that satisfies
+ *
+ *        x_dot = J*twist,
+ *
+ *        where x_dot is the time derivative of the unit dual quaternion
+ *        that represents the free-flying robot pose, and twist = vec8(csi),
+ *        where csi satisfies the dual quaternion propagation equation
+ *        xdot = (1/2)*csi*x.
+ *
+ * @param pose The unit dual quaternion that represents
+ *             the free-flying robot configuration.
+ * @param pose_derivative The time derivative of the robot configuration.
+ * @return The desired pose Jacobian derivative.
+ */
+MatrixXd DQ_FreeFlyingRobot::pose_jacobian_derivative(const DQ &pose_derivative) const
+{
+    return haminus8(0.5*pose_derivative);
+}
+
+}