SimObjects in fhsim_tutorial

Explanation of the SimObjects in fhsim_tutorial, from discretization to implementation. In this tutorial the SimObjects in fhsim_tutorial are elaborated with focus on the discretization of the system and the implementation as SimObjects.

Linear spring in 3D

Discretization

The input to the linear spring is the position at the ends A and B, \(p_A\) and \(p_B\). The outputs are the forces at A and B, \(F_A\) and \(F_B\). \(F_A[p_A(t),p_B(t)]\) and \(F_B[p_A(t),p_B(t)]\) must be implemented in the SimObject.

The length of the spring is found as

\(L=\sqrt{(p_{Ax}-p_{Bx})^2+(p_{Ay}-p_{By})^2+(p_{Az}-p_{Bz})^2} \tag{1}\)

The spring forces are then calculated as

\(L_{eff} = L-L_{relaxed} \tag{2}\)

where \(L_{relaxed}\) is the relaxed length of the spring. If \(k\) is the spring stiffness and \(L_{eff}\)>0, we can calculate the forces from the spring as

\(F_{Ax} = -k L_{eff}{p_{Ax}-p_{Bx}\over L}\tag{3a}\) \(F_{Ay} = -k L_{eff}{p_{Ay}-p_{By}\over L}\tag{3b}\) \(F_{Az} = -k L_{eff}{p_{Az}-p_{Bz}\over L}\tag{3c}\)

\(F_{Bx} = -F_{Ax}\tag{4a}\) \(F_{By} = -F_{Ay}\tag{4b}\) \(F_{Bz} = -F_{Az}\tag{4c}\)

Implementation

A thorough explanation of how to implement a SimObject in FhSim is out of scope here, see Create a new SimObject. The constructor of the linear spring is shown in the following code-block.

#include "CLinearSpring.h"
#include <cmath>
 
//Constructor for the linear spring class
CLinearSpring::CLinearSpring(std::string sSimObjectName, ISimObjectCreator* pCreator)
   : SimObject(sSimObjectName) {
#ifdef FH_VISUALIZATION
    m_iNumPoints = 2; // The number of points for the visualization.
#endif
    //Input ports
    pCreator->AddInport("PosA", 3, &m_pInPosA);
    pCreator->AddInport("PosB", 3, &m_pInPosB);
 
    // Register common computation function
    pCreator->RegisterCommonCalculation(COMMON_COMPUTATION_FUNCTION(CLinearSpring::CalcOutput), &m_CalcOutputs);
 
    // Output ports
    pCreator->AddOutport("ForceA", 3, PORT_FUNCTION(CLinearSpring::ForceA));
    pCreator->AddOutport("ForceB", 3, PORT_FUNCTION(CLinearSpring::ForceB));
 
    // Parameters
    pCreator->GetDoubleParam("Stiffness", &m_dStiffness);
    pCreator->GetDoubleParam("RelaxedLength", &m_dRelaxedLength);
}

CLinearSpring inherits the SimObject class, so the constructor must pass any arguments to the SimObject class. The constructor sets the SimObject parameters, as well as its signature in terms of states, input ports and output ports. In this case, the input ports are PosA and PosB, both of size 3. The output ports are ForceA and ForceB, both of size 3. The parameters Stiffness and RelaxedLength are set from the FhSim configuration file. Also note that the function CalcOutput is registered as a CommonComputation-function, which means that it will be calculated at most one time each time-step.

As the linear spring does not contain any states, no integration is needed, and no states are defined. The OdeFcn-function must still be defined, and it is defined as an empty function:

//Function for setting up the state space model.
void CLinearSpring::OdeFcn(const double dT, const double* const adX, double* const adXDot, const bool bIsMajorTimeStep)
{
     // Does nothing, as the object contains no states.
}

The value of the output ports are calculated in the CalcOutput function. This can be implemented as:

void CLinearSpring::CalcOutput(const double dT, const double* const adX) {
    const double* adPosA = m_pInPosA->GetPortValue(dT,adX);
    const double* adPosB = m_pInPosB->GetPortValue(dT,adX);
    double adDeltaPos[3];
    double dL = 0;
    for(int i = 0;i < 3;i++) {
        adDeltaPos[i] = adPosA[i]-adPosB[i];
        dL+=adDeltaPos[i]*adDeltaPos[i];
    }
    if (dL <= 0.0) {
        for (int i = 0; i < 3; i++) {
            m_adOutForceA[i] = 0.0;
            m_adOutForceB[i] = 0.0;
        }
    } else {
        dL = sqrt(dL);
        double dDeltaL = dL - m_dRelaxedLength;
        for (int i = 0; i < 3; i++) {
            m_adOutForceA[i] = -m_dStiffness * dDeltaL*adDeltaPos[i] / dL;
            m_adOutForceB[i] = -m_adOutForceA[i];
        }
    }
}

The constructor associates two functions with the output ports, namely ForceA and ForceB. These functions each call the common computation CalcOutput and then uses the results from this to set the output ports:

const double* CLinearSpring::ForceA( const double dT, const double* const adX ) {
    m_CalcOutputs->ComputeFunction(dT, adX);
    return m_adOutForceA;
}
 
const double* CLinearSpring::ForceB( const double dT, const double* const adX ) {
    m_CalcOutputs->ComputeFunction(dT, adX);
    return m_adOutForceB;
}

To include 3D-Visualization, RenderInit() and RenderUpdate must be defined. e.g. as:

#ifdef FH_VISUALIZATION
 
void CLinearSpring::RenderInit(Ogre::Root* const pOgreRoot, ISimObjectCreator* const pCreator) {
    auto scenemgr = pOgreRoot->getSceneManager("main");
    m_pLines = new C3DLine(scenemgr, Ogre::RenderOperation::OT_LINE_LIST,2);
}
 
 
void CLinearSpring::RenderUpdate(const double dT, const double *const adX) {
 
    const double* const adPos1In = m_pInPosA->GetPortValue(dT, adX);
    const double* const adPos2In = m_pInPosB->GetPortValue(dT, adX);
 
    // Assumes just values have changed, use 'setPoint' instead of 'addPoint'
    m_pLines->SetPoint(0,adPos1In[0],adPos1In[1],adPos1In[2]);
    m_pLines->SetPoint(1,adPos2In[0],adPos2In[1],adPos2In[2]);
 
    m_pLines->Update();
}
#endif

Mass in 3D (translations only)

Discretization

The mass object is a point mass with translations only. This means it has 6 states; three positions ( \(p_x\), \(p_y\), \(p_z\)) and three velocities ( \(v_x\), \(v_y\), \(v_z\)). It will have one input port, which is the forces acting on it ( \(F_x\), \(F_y\), \(F_z\)). The output will correspond to the six states. When taking gravity into account, its dynamics can be written as

\( \begin{align} \dot{p}_x & = v_x \text{, }\;\;\;\;\;\;\;\;\;\; \dot{v}_x = {1 \over m} F_x \tag{5a}\\ \dot{p}_y & = v_y \text{, }\;\;\;\;\;\;\;\;\;\; \dot{v}_y = {1 \over m} F_y \tag{5b}\\ \dot{p}_z & = v_z \text{, }\;\;\;\;\;\;\;\;\;\; \dot{v}_z = {1 \over m} F_z - g \tag{5c} \end{align} \)

where g is the acceleration of gravity.

Implementation

The constructor can be implemented as

#include "CMass.h"
 
CMass::CMass(std::string sSimObjectName, ISimObjectCreator* pCreator):SimObject(sSimObjectName) {
    //Input ports.
    pCreator->AddInport("Force", 3, &m_pInForce);
 
    // Output ports.
    pCreator->AddOutport("Pos", 3, PORT_FUNCTION(CMass::Position));
    pCreator->AddOutport("Vel", 3, PORT_FUNCTION(CMass::Velocity));
 
    // States
    m_IStatePos = pCreator->AddState("Pos", 3);
    m_IStateVel = pCreator->AddState("Vel", 3);
 
    // Parameters
    pCreator->GetDoubleParam("Mass", &m_dMass);
    pCreator->GetDoubleParam("g", &m_dg,0.0);
    if(m_dMass <= 0)
        pCreator->ReportParameterError("Mass", "Must be a real number greater than zero.");
 
#ifdef FH_VISUALIZATION
    pCreator->GetStringParam("Material", m_sMaterial, "Simple/Black");
    pCreator->GetStringParam("Mesh", m_sMeshName, "fhSphere.mesh");
    pCreator->GetDoubleParam("Scale",&m_dScale, 1.0);
#endif
}

In contrast to the linear spring object, the mass object contains six states. The OdeFcn must therefore calculate the time derivatives of the states:

void CMass::OdeFcn(const double dT, const double* const adX, double* const adXDot, const bool bIsMajorTimeStep) {
    const double* const adForceIn = m_pInForce->GetPortValue(dT,adX);
 
    for (int i = 0; i<3 ; i++) {
        adXDot[m_IStatePos + i] =  adX[m_IStateVel + i];
        adXDot[m_IStateVel + i] = adForceIn[i] / m_dMass ;
 
        // Adding acceleration of gravity
        if (i==2)
            adXDot[m_IStateVel + i] += -m_dg;
    }
}

The output ports (position and velocity) are returned by the functions (Position and Velocity):

const double* CMass::Position( const double dT, const double* const adX ) {
    return adX + m_IStatePos;
}
 
const double* CMass::Velocity( const double dT, const double* const adX ) {
    return adX + m_IStateVel;
}

To add visualization, the functions RenderInit and RenderUpdate must be implemented. Note that all reference to Ogre3D must only be compiled when FH_VISUALIZATION is defined. This enables the same source code to be compiled completely without visualization, presumably giving a smaller memory footprint, better use of the different computer caches and a faster simulation. The rendering functions:

#ifdef FH_VISUALIZATION
void CMass::RenderInit(Ogre::Root* const pOgreRoot, ISimObjectCreator* const pCreator) {
    m_pSceneMgr     = pOgreRoot->getSceneManager("main");
    m_pRenderNode   = m_pSceneMgr->getRootSceneNode()->createChildSceneNode( m_SimObjectName + "FollowNode" );
    m_pRenderEntity = m_pSceneMgr->createEntity( m_SimObjectName + "Entity",  m_sMeshName);
    m_pRenderEntity->setMaterialName(m_sMaterial);
    m_pRenderNode->attachObject( m_pRenderEntity );
    m_pRenderNode->scale(m_dScale,m_dScale,m_dScale);
}
 
void CMass::RenderUpdate(const double T, const double* const X) {
    m_pRenderNode->setPosition(Ogre::Vector3(X[m_IStatePos + 0],X[m_IStatePos + 1],X[m_IStatePos + 2]));
}
#endif

Defining the simulation in an input file

To simulate the two SimObjects connected to each other, the input file MassLinearSpring.xml is written:

<Contents>
    <OBJECTS>
        <Lib
            LibName="fhsim_tutorial"
                SimObject="Cable/LinearSpring"
                Name="S"
                Stiffness="100"
                RelaxedLength="10"
        />
        <Lib
                LibName="fhsim_tutorial"
                SimObject="Body/Mass"
                Name="B"
                Scale="1.0"
                Mass="1.0"
                g="-9.81"
                Material="Simple/Red"
            />
    </OBJECTS>
    <INTERCONNECTIONS>
        <Connection
                S.PosB="B.Pos"
                B.Force="S.ForceB"
                S.PosA="0,0,-10"
        />
    </INTERCONNECTIONS>
    <INITIALIZATION>
        <InitialCondition
            B.Pos="0,0,-5"
            B.Vel="0,0,0"
        />
    </INITIALIZATION>
    <INTEGRATION>
            <Engine
                IntegratorMethod="2"
                NumCores="1"
                TOutput="0, 0:0.1:10, 100"
                LogStates ="1"
                stepsize ="0"
                HMax="0.002"
                HMin="0.0000001"
                AbsTol="1e-3" RelTol="1e-3"
                UseRSSNormInsteadOfInfNorm="0"
                FileOutput="object.B:states"
        />
    </INTEGRATION>
</Contents>

When compiling the project with visualization enabled and running FhRtVis.exe with MassLinearSpring.xml as input, the visualization should look something like the figure below.

Visualization of the connected linear spring and damper in a real-time simulation