From f4017c13db8c9fe338e836d7261f5cd57c05908d Mon Sep 17 00:00:00 2001 From: Murilo Marinho Date: Wed, 23 Sep 2026 11:54:27 +0100 Subject: [PATCH] feat(modeling): add SerialManipulatorSimulatorFriendly under marinholab::sas::core::modeling Introduce SerialManipulatorSimulatorFriendly, a serial-manipulator kinematics model with configurable per-joint offsets and actuation types, in the namespace marinholab::sas::core::modeling. Ported from sas_robot_driver_gazebo (namespace DQ_robotics), where it lived as M3_SerialManipulatorSimulatorFriendly; the class name drops the M3_ prefix. dqrobotics types/functions are pulled into the namespace via scoped using-declarations (the original relied on living inside namespace DQ_robotics). Behavior is unchanged; the pose Jacobian agrees with the finite-difference derivative of raw_fkm. Co-authored-by: openhands --- CMakeLists.txt | 1 + .../serial_manipulator_simulator_friendly.hpp | 127 ++++++++++ src/serial_manipulator_simulator_friendly.cpp | 229 ++++++++++++++++++ 3 files changed, 357 insertions(+) create mode 100644 include/marinholab/sas/core/modeling/serial_manipulator_simulator_friendly.hpp create mode 100644 src/serial_manipulator_simulator_friendly.cpp diff --git a/CMakeLists.txt b/CMakeLists.txt index 9f23c9f..d0daad7 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -55,6 +55,7 @@ add_library(marinholab_sas_core src/sas_robot_driver_example.cpp src/eigen3_std_conversions.cpp src/sas_thread_manager.cpp + src/serial_manipulator_simulator_friendly.cpp ) add_library(marinholab::sas::core ALIAS marinholab_sas_core) diff --git a/include/marinholab/sas/core/modeling/serial_manipulator_simulator_friendly.hpp b/include/marinholab/sas/core/modeling/serial_manipulator_simulator_friendly.hpp new file mode 100644 index 0000000..fe54d05 --- /dev/null +++ b/include/marinholab/sas/core/modeling/serial_manipulator_simulator_friendly.hpp @@ -0,0 +1,127 @@ +#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 . +# +# ################################################################ +# +# Author: Murilo M. Marinho, email: murilomarinho@ieee.org +# +# ################################################################*/ + +/** + * @file serial_manipulator_simulator_friendly.hpp + * @brief A serial-manipulator kinematics model with configurable per-joint + * offsets and actuation types. + */ + +#include +#include +#include + +#include + +namespace marinholab::sas::core::modeling +{ + +// The dqrobotics types this model builds on. (The Eigen types used in the +// signatures, e.g. MatrixXd/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; + +/** + * @brief A serial manipulator whose joints carry explicit pre/post offsets. + * + * Each joint contributes a dual-quaternion transformation of the form + * @c offset_before_ * actuation(q) * offset_after_, which lets the model + * represent sensor frames and joint offsets that a plain + * @ref DQ_SerialManipulator would not. Supports both revolute (R) and + * prismatic (T) joints about any principal axis. + */ +class SerialManipulatorSimulatorFriendly: public DQ_SerialManipulator +{ +public: + /** @brief The actuation type and axis of a single joint. */ + enum class ActuationType{ + RZ, ///< Revolution about the z-axis. + RY, ///< Revolution about the y-axis. + RX, ///< Revolution about the x-axis. + TZ, ///< Translation along the z-axis. + TY, ///< Translation along the y-axis. + TX ///< Translation along the x-axis. + }; +protected: + std::vector offset_before_; + std::vector offset_after_; + std::vector actuation_types_; + + DQ _get_w(const int& ith) const; + DQ _joint_transformation(const double& q, const int& ith) const; +public: + + + SerialManipulatorSimulatorFriendly()=delete; + /** + * @brief Construct the manipulator. + * + * @param offset_before Per-joint dual-quaternion offset applied before actuation. + * @param offset_after Per-joint dual-quaternion offset applied after actuation. + * @param actuation_types Per-joint actuation type and axis. + * + * @throws std::runtime_error if the three vectors do not have equal size. + */ + SerialManipulatorSimulatorFriendly(const std::vector& offset_before, + const std::vector& offset_after, + const std::vector& actuation_types); + + using DQ_SerialManipulator::raw_pose_jacobian; + using DQ_SerialManipulator::raw_pose_jacobian_derivative; + using DQ_SerialManipulator::raw_fkm; + + /** + * @brief Joint types this model can represent. + * @return A vector containing @c DQ_JointType::REVOLUTE. + */ + std::vector get_supported_joint_types() const override; + + /** + * @brief Raw pose Jacobian of the chain up to a given link. + * @param q_vec Joint configuration vector. + * @param to_ith_link Index of the terminal link. + * @return An 8 x (to_ith_link+1) dual-quaternion pose Jacobian. + */ + MatrixXd raw_pose_jacobian(const VectorXd& q_vec, const int& to_ith_link) const override; + /** + * @brief Time derivative of the raw pose Jacobian. + * @param q Joint configuration vector. + * @param q_dot Joint velocity vector. + * @param to_ith_link Index of the terminal link. + * @return An 8 x (to_ith_link+1) Jacobian-derivative matrix. + */ + MatrixXd raw_pose_jacobian_derivative(const VectorXd& q, const VectorXd& q_dot, const int& to_ith_link) const override; + /** + * @brief Raw forward kinematics of the chain up to a given link. + * @param q_vec Joint configuration vector. + * @param to_ith_link Index of the terminal link. + * @return The dual-quaternion pose of the terminal link. + */ + DQ raw_fkm(const VectorXd &q_vec, const int &to_ith_link) const override; +}; + +} diff --git a/src/serial_manipulator_simulator_friendly.cpp b/src/serial_manipulator_simulator_friendly.cpp new file mode 100644 index 0000000..c66412f --- /dev/null +++ b/src/serial_manipulator_simulator_friendly.cpp @@ -0,0 +1,229 @@ +/* +# 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 . +# +# ################################################################ +# +# Author: Murilo M. Marinho, email: murilomarinho@ieee.org +# +# ################################################################*/ + +/** + * @file serial_manipulator_simulator_friendly.cpp + * @brief Implementation of SerialManipulatorSimulatorFriendly. + * + * The implementation follows the standard dual-quaternion serial-manipulator + * construction: each joint contributes + * @c offset_before_(i) * actuation(q_i) * offset_after_(i), and the pose + * Jacobian columns are built from the joint axis transformed by the + * intermediate poses (see @ref DQ_SerialManipulator in dqrobotics-cpp). + */ + +#include + +namespace marinholab::sas::core::modeling +{ + +// Free functions/constants from dqrobotics used by the implementation. (The +// types DQ/VectorXd/MatrixXd are already in scope via the header / the +// dqrobotics headers' global-namespace injection.) +using DQ_robotics::Ad; +using DQ_robotics::C8; +using DQ_robotics::E_; +using DQ_robotics::haminus8; +using DQ_robotics::hamiplus8; +using DQ_robotics::i_; +using DQ_robotics::j_; +using DQ_robotics::k_; +using DQ_robotics::vec8; + +SerialManipulatorSimulatorFriendly::SerialManipulatorSimulatorFriendly(const std::vector &offset_before, + const std::vector &offset_after, + const std::vector& actuation_types): + DQ_SerialManipulator(actuation_types.size()), + offset_before_(offset_before), + offset_after_(offset_after), + actuation_types_(actuation_types) +{ + if(offset_before_.size() != actuation_types_.size() || + offset_after_.size() != actuation_types_.size() ) + throw std::runtime_error("Size issue"); + + // The base class only resizes the limit vectors, leaving them + // uninitialized. Initialize them to a wide range so that + // get_lower_q_limit()/get_upper_q_limit() never return garbage and the + // joint-limit constraints stay feasible until the caller sets real + // limits with set_lower_q_limit()/set_upper_q_limit(). + // ``kDefaultJointLimit`` is a large (practically unbounded) value in + // radians, the unit used for joint positions throughout dqrobotics. + static constexpr double kDefaultJointLimit = 10.0; + lower_q_limit_ = VectorXd::Constant(actuation_types_.size(), -kDefaultJointLimit); + upper_q_limit_ = VectorXd::Constant(actuation_types_.size(), kDefaultJointLimit); +} + +DQ SerialManipulatorSimulatorFriendly::_joint_transformation(const double &q, const int &ith) const +{ + // Returns the dual-quaternion joint transformation + // offset_before_(ith) * actuation(q) * offset_after_(ith) + // where actuation(q) is the dual quaternion corresponding to the + // joint's actuation type at value q. + const auto& before = offset_before_.at(ith); + const auto& after = offset_after_.at(ith); + + DQ actuation; + + switch(actuation_types_.at(ith)) + { + case ActuationType::RZ: + actuation = cos(q/2.0) + k_*sin(q/2.0); + break; + case ActuationType::RY: + actuation = cos(q/2.0) + j_*sin(q/2.0); + break; + case ActuationType::RX: + actuation = cos(q/2.0) + i_*sin(q/2.0); + break; + case ActuationType::TZ: + actuation = 1 + 0.5*E_*k_*q; + break; + case ActuationType::TY: + actuation = 1 + 0.5*E_*j_*q; + break; + case ActuationType::TX: + actuation = 1 + 0.5*E_*i_*q; + break; + } + + return before*actuation*after; +} + + +/** + * @brief Returns the spatial axis (as a dual quaternion) of the joint. + * + * For a revolute joint this is the unit quaternion of the rotation axis; + * for a prismatic joint it is the dual quaternion of the translation + * direction (i.e. @c E_*axis). + * + * @param ith Joint index. + * @return The dual quaternion representing the joint axis. + * @throws std::runtime_error if the joint's actuation type is invalid. + */ +DQ SerialManipulatorSimulatorFriendly::_get_w(const int &ith) const +{ + switch(actuation_types_.at(ith)) + { + case ActuationType::RZ: + return k_; + break; + case ActuationType::RY: + return j_; + break; + case ActuationType::RX: + return i_; + break; + case ActuationType::TZ: + return E_*k_; + break; + case ActuationType::TY: + return E_*j_; + break; + case ActuationType::TX: + return E_*i_; + break; + } + throw std::runtime_error("Invalid actuation"); +} + +DQ SerialManipulatorSimulatorFriendly::raw_fkm(const VectorXd& q_vec, const int& to_ith_link) const +{ + _check_q_vec(q_vec); + _check_to_ith_link(to_ith_link); + + DQ q(1); + int j = 0; + for (int i = 0; i < (to_ith_link+1); i++) { + q = q * _joint_transformation(q_vec(i-j), i); + } + return q; +} + + +MatrixXd SerialManipulatorSimulatorFriendly::raw_pose_jacobian(const VectorXd &q_vec, const int &to_ith_link) const +{ + _check_q_vec(q_vec); + _check_to_ith_link(to_ith_link); + + MatrixXd J = MatrixXd::Zero(8,to_ith_link+1); + DQ x_effector = raw_fkm(q_vec,to_ith_link); + + DQ x(1); + + for(int i=0;i SerialManipulatorSimulatorFriendly::get_supported_joint_types() const +{ + return {DQ_JointType::REVOLUTE}; +} + +}