PlatformInertia.cpp

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
 */
 
#include "PlatformInertia.h"
 
#include <filesystem>
#include <stdexcept>
 
#include <ArmarXCore/core/PackagePath.h>
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
#include <ArmarXCore/core/logging/Logging.h>
#include <ArmarXCore/core/util/StringHelpers.h>
 
#if NAVIGATION_SIMULATION_SIMOX_CONTROL
 
#include <VirtualRobot/Robot.h>
#include <VirtualRobot/RobotNodeSet.h>
#include <VirtualRobot/XML/RobotIO.h>
 
#include <simox/control/dynamics/RBDLModel.h>
#include <simox/control/impl/simox/dynamics/VirtualRobotRBDLModel.h>
 
#endif
 
namespace armarx::navigation::simulation
{
 
#if NAVIGATION_SIMULATION_SIMOX_CONTROL
 
    PlatformInertia::PlatformInertia(const Parameters& params) : params_(params)
    {
        const std::string filename =
            armarx::PackagePath(params_.robotPackage, params_.robotFile).toSystemPath();
 
        ARMARX_CHECK(std::filesystem::exists(filename))
            << "Robot model " << QUOTED(filename) << " does not exist.";
 
        // eStructure skips visualization and collision meshes; <Physics> is still parsed, which
        // is all the dynamics model needs.
        robot_ = VirtualRobot::RobotIO::loadRobot(filename, VirtualRobot::RobotIO::eStructure);
        ARMARX_CHECK_NOT_NULL(robot_) << "Failed to load " << QUOTED(filename);
 
        ARMARX_CHECK(robot_->hasConfiguration(params_.configuration))
            << "Robot has no configuration " << QUOTED(params_.configuration) << ".";
 
        // Must happen before the RBDL model is built: simox-control lumps the bodies using the
        // transforms as they are at construction time.
        ARMARX_CHECK(robot_->setToConfiguration(params_.configuration));
 
        const VirtualRobot::RobotNodeSetPtr rns = robot_->getRobotNodeSet(params_.nodeSet);
        ARMARX_CHECK_NOT_NULL(rns) << "Robot has no node set " << QUOTED(params_.nodeSet) << ".";
 
        // rnsBodies = nullptr lumps every mass below the platform onto the platform bodies,
        // which is exactly the whole-robot inertia we are after.
        model_ = std::make_unique<simox::control::simox::dynamics::VirtualRobotRBDLModel>(rns);
 
        ARMARX_CHECK_EQUAL(model_->getnDoF(), 3)
            << "Expected a 3-DoF reduced platform model from node set "
            << QUOTED(params_.nodeSet) << " but got " << model_->getnDoF()
            << " DoF. The [x, y, yaw] block extraction below assumes indices 0, 1, 2.";
 
        // The translation/rotation cross-terms come from the centre of mass being offset from
        // the yaw axis. On ARMAR-7 they are larger than the yaw inertia itself, so they must
        // not be dropped when sizing acceleration limits.
        const Eigen::Matrix3d M = massMatrix(Eigen::Vector3d::Zero());
 
        ARMARX_IMPORTANT << "Platform dynamics from " << QUOTED(filename) << " in configuration "
                         << QUOTED(params_.configuration) << ": mass " << M(0, 0)
                         << " kg, yaw inertia " << M(2, 2) << " kg m^2.\n"
                         << "Mass matrix [x, y, yaw] (kg, kg, kg m^2):\n"
                         << M;
    }
 
    PlatformInertia::~PlatformInertia() = default;
 
    Eigen::Matrix3d
    PlatformInertia::massMatrix(const Eigen::Vector3d& q)
    {
        // q is already [m, m, rad]; simox-control builds the RBDL model in metres but never
        // scales q itself, so the caller owes us metres for the prismatic DoFs.
        return model_->getInertiaMatrix(q);
    }
 
    Eigen::Vector3d
    PlatformInertia::bias(const Eigen::Vector3d& q, const Eigen::Vector3d& qdot)
    {
        return model_->getInverseDynamics(q, qdot, Eigen::Vector3d::Zero());
    }
 
    double
    PlatformInertia::totalMass()
    {
        return massMatrix(Eigen::Vector3d::Zero())(0, 0);
    }
 
    double
    PlatformInertia::yawInertia()
    {
        return massMatrix(Eigen::Vector3d::Zero())(2, 2);
    }
 
#else
 
    PlatformInertia::PlatformInertia(const Parameters& /*params*/)
    {
        throw std::runtime_error(
            "The omni-wheel torque model needs the platform inertia, which requires "
            "SimoxControl. This build of armarx_navigation was configured without it. Use a "
            "different `model` in PlatformDynamics.json, or rebuild with SimoxControl available.");
    }
 
    PlatformInertia::~PlatformInertia() = default;
 
    Eigen::Matrix3d
    PlatformInertia::massMatrix(const Eigen::Vector3d&)
    {
        return Eigen::Matrix3d::Identity();
    }
 
    Eigen::Vector3d
    PlatformInertia::bias(const Eigen::Vector3d&, const Eigen::Vector3d&)
    {
        return Eigen::Vector3d::Zero();
    }
 
    double
    PlatformInertia::totalMass()
    {
        return 0.0;
    }
 
    double
    PlatformInertia::yawInertia()
    {
        return 0.0;
    }
 
#endif
 
} // namespace armarx::navigation::simulation