Program Listing for File SOADynUtils.h

↰ Return to documentation for file (include/Karana/SOADyn/SOADynUtils.h)

/*
 * Copyright (c) 2024-2026 Karana Dynamics Pty Ltd. All rights reserved.
 *
 * NOTICE TO USER:
 *
 * This source code and/or documentation (the "Licensed Materials") is
 * the confidential and proprietary information of Karana Dynamics Inc.
 * Use of these Licensed Materials is governed by the terms and conditions
 * of a separate software license agreement between Karana Dynamics and the
 * Licensee ("License Agreement"). Unless expressly permitted under that
 * agreement, any reproduction, modification, distribution, or disclosure
 * of the Licensed Materials, in whole or in part, to any third party
 * without the prior written consent of Karana Dynamics is strictly prohibited.
 *
 * THE LICENSED MATERIALS ARE PROVIDED "AS IS" WITHOUT WARRANTY OF ANY KIND.
 * KARANA DYNAMICS DISCLAIMS ALL WARRANTIES, EXPRESS OR IMPLIED, INCLUDING
 * BUT NOT LIMITED TO WARRANTIES OF MERCHANTABILITY, NON-INFRINGEMENT, AND
 * FITNESS FOR A PARTICULAR PURPOSE.
 *
 * IN NO EVENT SHALL KARANA DYNAMICS BE LIABLE FOR ANY DAMAGES WHATSOEVER,
 * INCLUDING BUT NOT LIMITED TO LOSS OF PROFITS, DATA, OR USE, EVEN IF
 * ADVISED OF THE POSSIBILITY OF SUCH DAMAGES, WHETHER IN CONTRACT, TORT,
 * OR OTHERWISE ARISING OUT OF OR IN CONNECTION WITH THE LICENSED MATERIALS.
 *
 * U.S. Government End Users: The Licensed Materials are a "commercial item"
 * as defined at 48 C.F.R. 2.101, and are provided to the U.S. Government
 * only as a commercial end item under the terms of this license.
 *
 * Any use of the Licensed Materials in individual or commercial software must
 * include, in the user documentation and internal source code comments,
 * this Notice, Disclaimer, and U.S. Government Use Provision.
 */

/**
 * @file
 * @brief Contains the declarations for the FrameToFrame class.
 */

#pragma once

#include "Karana/KCore/Id.h"
#include "Karana/Math/HomTran.h"
#include "Karana/SOADyn/Defs.h"
#include <array>
#include <cstddef>

namespace Karana::Dynamics {

    namespace km = Karana::Math;
    namespace kc = Karana::Core;

    class SubGraph;

    /** Total number of dofs allowed in the multibody system */
#define MAX_DOFS 200
    /** Total number of external force nodes allowed in the multibody system */
#define MAX_NODES 30

    struct ModelPool;

    /** Struct for passing state related to and from ModelPool */
    struct IODataPool {

        // km::Vec Q(MAX_DOFS);

        /** the Q generalized coordinates */
        Eigen::Matrix<double, MAX_DOFS, 1> Q;
        /** the U generalized velocity coordinates */
        Eigen::Matrix<double, MAX_DOFS, 1> U;
        /** the T generalized force coordinates */
        Eigen::Matrix<double, MAX_DOFS, 1> T;
        /** the Udot generalized acceleration coordinates */
        Eigen::Matrix<double, MAX_DOFS, 1> Udot; // NOLINT(readability-identifier-naming)
        // km::SpatialForce external_force[MAX_NODES];
        /** the expternal spatial force values */
        std::array<km::SpatialForce, MAX_NODES> external_force;
        // km::Vec3 gravity_accel;
    };

    /**
     * @brief Create a ModelPool instance for a subgraph
     *
     * @param sg The SubGrapht to create a ModelPool for.
     * @return The newly created ModelPool instance.
     */
    ModelPool createModelPool(const SubGraph &sg);

    /**
     * @brief Create a IODataPool instance for a subgraph
     *
     * @param sg The SubGrapht to create a IODataPool for.
     * @return The newly created IODataPool instance.
     */
    IODataPool createIODataPool(const SubGraph &sg);

    /**
     * @brief Evaluate ModelPool based forward dynamics
     *
     * @param mpool The ModelPool to evaluate.
     * @param inputs The inputs to evaluate the forward dynamics at.
     */
    void treeForwardDynamicsPool(ModelPool &mpool, IODataPool &inputs);

} // namespace Karana::Dynamics