computeItemTest.py

You can view and download this file on Github: computeItemTest.py

#+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
# This is an EXUDYN example
#
# Details:  mbs.ComputeItem computes what the solver computes for one item, at the current state (#2779) - for
#           tests and for the debugging of a model or of a user's item; exudyn.advancedUtilities.NumericalJacobian
#           gives the numerical derivative to compare with. A double pendulum of two rigid bodies, the first on
#           a revolute joint to the ground, the second on a spring-damper:
#           (1) mbs.ComputeItem(item) lists what applies to the item;
#           (2) the mass matrix and the right-hand side of a body, the forces of the spring-damper on its markers;
#           (3) the algebraic equations of the joint and its constraint Jacobian, which equals the numerical
#               derivative of the equations, and its reaction forces C_q^T lambda;
#           (4) the position Jacobian of a body at a point, the derivative of its velocity by the velocities;
#           (5) the kinematics of a marker, the rotation Jacobian of a node.
#
# Author:   Johannes Gerstmayr
# Date:     2026-10-02
#
# Copyright:This file is part of Exudyn. Exudyn is free software. You can redistribute it and/or modify it under the terms of the Exudyn license. See 'LICENSE.txt' for more details.
#
#+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++

import exudyn as exu
from exudyn.utilities import * #includes itemInterface and rigidBodyUtilities
from exudyn.advancedUtilities import NumericalJacobian, ItemODE2Coordinates
import numpy as np

testIsActive = exu.sys.get('testIsActive', False)
IC = exu.ComputeItemType

SC = exu.SystemContainer()
mbs = SC.AddSystem()
oGround = mbs.AddObject(ObjectGround())
inertia = InertiaCuboid(density=1000, sideLengths=[0.4, 0.05, 0.05])
b0 = mbs.CreateRigidBody(inertia=inertia, referencePosition=[0.2, 0, 0], nodeType=exu.NodeType.RotationRxyz,
                         initialAngularVelocity=[0, 0, 1], gravity=[0, -9.81, 0], returnDict=True)
b1 = mbs.CreateRigidBody(inertia=inertia, referencePosition=[0.6, 0, 0], nodeType=exu.NodeType.RotationRxyz,
                         gravity=[0, -9.81, 0], returnDict=True)
oJoint = mbs.CreateRevoluteJoint(itemNumbers=[oGround, b0['bodyNumber']], position=[0, 0, 0], axis=[0, 0, 1])
oSpring = mbs.CreateSpringDamper(itemNumbers=[b0['bodyNumber'], b1['bodyNumber']], localPosition0=[0.2, 0, 0],
                                 localPosition1=[-0.2, 0, 0], stiffness=1e3, damping=1)
p = [0.2, 0, 0] #a point of the first body
marker = mbs.AddMarker(MarkerBodyRigid(bodyNumber=b0['bodyNumber'], localPosition=p))
mbs.Assemble()

simulationSettings = exu.SimulationSettings()
simulationSettings.timeIntegration.newton.useModifiedNewton = False #Just for the test; modified Newton is usually faster
simulationSettings.timeIntegration.numberOfSteps = 100
simulationSettings.timeIntegration.endTime = 0.2
simulationSettings.solution.file.write = False
mbs.SolveDynamic(simulationSettings) #some state that is not the initial one

body = b0['bodyNumber']
#(1) what applies
exu.Print('ComputeItem applies to the body:', [str(t) for t in mbs.ComputeItem(body)])
exu.Print('ComputeItem applies to the joint:', [str(t) for t in mbs.ComputeItem(oJoint)])

#(2) the body and the spring-damper
massMatrix = mbs.ComputeItem(body, IC.MassMatrix)
lhsBody = mbs.ComputeItem(body, IC.ODE2LHS)
lhsSpring = mbs.ComputeItem(oSpring, IC.ODE2LHS)
exu.Print('mass of the body from its mass matrix:', massMatrix[0, 0], ', expected', inertia.Mass())

#(3) the joint
equations = mbs.ComputeItem(oJoint, IC.AlgebraicEquations)
constraintJacobian = mbs.ComputeItem(oJoint, IC.ConstraintJacobian)
coordinates = ItemODE2Coordinates(mbs, oJoint)
numerical = NumericalJacobian(mbs, lambda: mbs.ComputeItem(oJoint, IC.AlgebraicEquations), coordinates)
errorCq = np.abs(constraintJacobian - numerical).max()
exu.Print('joint equations', np.round(equations, 8), ', C_q against its numerical derivative:', errorCq)
reactionForces = mbs.ComputeItem(oJoint, IC.ReactionForces)

#(4) the position Jacobian at a point of the body
positionJacobian = mbs.ComputeItem(body, IC.PositionJacobian, localPosition=p)
numerical = NumericalJacobian(mbs, lambda: mbs.GetObjectOutputBody(body, exu.OutputVariableType.Velocity, localPosition=p),
                              ItemODE2Coordinates(mbs, body), velocities=True)
errorJpos = np.abs(positionJacobian - numerical).max()
exu.Print('position Jacobian against the derivative of the velocity:', errorJpos)

#(5) a marker and a node
kinematics = mbs.ComputeItem(marker, IC.Kinematics)
exu.Print('marker position', np.round(kinematics['position'], 6))
rotationJacobian = mbs.ComputeItem(b0['nodeNumber'], IC.RotationJacobian)

testResult = (np.abs(massMatrix).sum() + np.abs(lhsBody).sum() + np.abs(lhsSpring).sum() + np.abs(constraintJacobian).sum()
              + np.abs(reactionForces).sum() + np.abs(kinematics['position']).sum() + np.abs(rotationJacobian).sum())
exu.Print('errors (both shall be small):', errorCq < 1e-6, errorJpos < 1e-8)
exu.Print('solution of computeItemTest=', testResult)
exu.sys['testResult'] = testResult