serial_manipulator_simulator_friendly.hpp

Go to the documentation of this file.

#pragma once
/*
# Copyright (c) 2016-2025 Murilo Marques Marinho
#
#    This file is part of sas_core.
#
#    sas_core 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.
#
#    sas_core 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 sas_core.  If not, see <https://www.gnu.org/licenses/>.
#
# ################################################################
#
#   Author: Murilo M. Marinho, email: murilomarinho@ieee.org
#
# ################################################################*/

 
#include <memory>
#include <stdexcept>
#include <vector>
 
#include <dqrobotics/robot_modeling/DQ_SerialManipulator.h>
 
namespace marinholab::sas::core::modeling
{
 
// The dqrobotics types this model builds on. (The Eigen types used in the
// signatures, e.g. Eigen::MatrixXd/Eigen::VectorXd, are injected into the global namespace
// by the dqrobotics headers.)
using DQ_robotics::DQ;
using DQ_robotics::DQ_SerialManipulator;
using DQ_robotics::DQ_JointType;

class SerialManipulatorSimulatorFriendly: public DQ_SerialManipulator
{
public:
    enum class ActuationType{
        RZ, 
        RY, 
        RX, 
        TZ, 
        TY, 
        TX  
    };
protected:
    std::vector<DQ> offset_before_;
    std::vector<DQ> offset_after_;
    std::vector<ActuationType> actuation_types_;
 
    DQ _get_w(const int& ith) const;
    DQ _joint_transformation(const double& q, const int& ith) const;
public:
 
 
    SerialManipulatorSimulatorFriendly()=delete;
    SerialManipulatorSimulatorFriendly(const std::vector<DQ>& offset_before,
                                       const std::vector<DQ>& offset_after,
                                       const std::vector<ActuationType>& actuation_types);
 
    using DQ_SerialManipulator::raw_pose_jacobian;
    using DQ_SerialManipulator::raw_pose_jacobian_derivative;
    using DQ_SerialManipulator::raw_fkm;

    std::vector<DQ_JointType> get_supported_joint_types() const override;

    Eigen::MatrixXd raw_pose_jacobian(const Eigen::VectorXd& q_vec, const int& to_ith_link) const override;
    Eigen::MatrixXd raw_pose_jacobian_derivative(const Eigen::VectorXd& q, const Eigen::VectorXd& q_dot, const int& to_ith_link) const override;
    DQ raw_fkm(const Eigen::VectorXd &q_vec, const int &to_ith_link) const override;
};
 
}