PlatformInertia.h

Go to the documentation of this file.

/**
 * This file is part of ArmarX.
 *
 * ArmarX is free software; you can redistribute it and/or modify
 * it under the terms of the GNU General Public License version 2 as
 * published by the Free Software Foundation.
 *
 * ArmarX 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 General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with this program. If not, see <http://www.gnu.org/licenses/>.
 *
 * @author     Fabian Reister ( fabian dot reister at kit dot edu )
 * @date       2026
 * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
 *             GNU General Public License
 */
 
#pragma once
 
#include <memory>
#include <string>
 
#include <Eigen/Core>
 
namespace VirtualRobot
{
    class Robot;
    using RobotPtr = std::shared_ptr<Robot>;
} // namespace VirtualRobot
 
namespace simox::control::dynamics
{
    class RBDLModel;
} // namespace simox::control::dynamics
 
namespace armarx::navigation::simulation
{
 
    /**
     * @brief Rigid-body inertia of the mobile platform, from the robot model.
     *
     * Wraps a *reduced* simox-control RBDL model built on the three virtual platform joints
     * (`X_Platform`, `Y_Platform`, `Yaw_Platform` — the `PlatformPlanning` node set). Everything
     * above the platform is lumped rigidly onto the yaw body at the robot's configuration, so
     * the result is a 3x3 mass matrix in `[x, y, yaw]`.
     *
     * The reduced model is deliberate: ARMAR-7 uses four-bar joints (`Knee`, `Ankle`) and
     * hemisphere joints (wrists) that RBDL cannot represent as ordinary joints. None of them is
     * in `PlatformPlanning`, so they are only ever lumped as rigid bodies and never need a
     * Jacobian.
     *
     * @note The robot must be put into its configuration *before* the RBDL model is built —
     *       simox-control freezes the joint transforms at construction time. The constructor
     *       does this in the right order.
     */
    class PlatformInertia
    {
    public:
        struct Parameters
        {
            std::string robotPackage{"armar7_models"};
 
            std::string robotFile{"robotmodel/armar7/Armar7.xml"};
 
            /// Node set holding exactly the platform DoFs, in [x, y, yaw] order.
            std::string nodeSet{"PlatformPlanning"};
 
            /// Robot configuration to lump the upper body at.
            std::string configuration{"Home"};
        };
 
        explicit PlatformInertia(const Parameters& params);
        ~PlatformInertia();
 
        PlatformInertia(const PlatformInertia&) = delete;
        PlatformInertia& operator=(const PlatformInertia&) = delete;
 
        /**
         * @brief Joint-space mass matrix at platform pose `q`.
         *
         * @param q `[x, y, yaw]` with **x and y in metres** and yaw in rad.
         * @return 3x3, SI: `[kg, kg, kg m^2]` on the diagonal. Depends on yaw only — the two
         *         prismatic DoFs are translation-invariant.
         */
        Eigen::Matrix3d massMatrix(const Eigen::Vector3d& q);
 
        /**
         * @brief Coriolis, centrifugal and gravity terms at the platform DoFs.
         *
         * This is `InverseDynamics(q, qdot, 0)`, i.e. the generalized force needed to hold the
         * current velocity. It is non-zero because the centre of mass is offset ~150 mm from
         * the yaw axis. Gravity contributes nothing to purely horizontal DoFs.
         */
        Eigen::Vector3d bias(const Eigen::Vector3d& q, const Eigen::Vector3d& qdot);
 
        /// Total mass of everything lumped onto the platform [kg], i.e. `massMatrix(0)(0, 0)`.
        double totalMass();
 
        /// Moment of inertia about the platform's yaw axis [kg m^2], at zero yaw.
        double yawInertia();
 
    private:
        Parameters params_;
 
        VirtualRobot::RobotPtr robot_;
 
        std::unique_ptr<simox::control::dynamics::RBDLModel> model_;
    };
 
} // namespace armarx::navigation::simulation