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