<!-- GENERATED by tools/generators/examplesDocsEmitter.py from python/ - do not edit -->
(testmodels-coordinatevectorconstraint)=
# coordinateVectorConstraint.py

You can view and download this file on Github: [coordinateVectorConstraint.py](https://github.com/jgerstmayr/EXUDYN/tree/master/python/TestModels/coordinateVectorConstraint.py)

```{code-block} python
:linenos:

#+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
# This is an EXUDYN example
#
# Details:  Example of double pendulum with Mass points and CoordinateVectorConstraint;
#
# Author:   Johannes Gerstmayr
# Date:     2022-03-17
#
# 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
import exudyn.graphics as graphics
import numpy as np

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

SC = exu.SystemContainer()
mbs = SC.AddSystem()

doublePendulum = True
withUserFunction = True

L = 0.8 #length of arm
mass = 2.5
g = 9.81

r = 0.05 #just for graphics
d = r/2

#add ground object and mass point:
sizeRect = 1.2*L*(1+int(doublePendulum))
#graphicsBackground = GraphicsDataRectangle(-sizeRect,-sizeRect, sizeRect, 0.2*L, [1,1,1,1]) #for appropriate zoom
graphicsBackground = graphics.CheckerBoard(point=[0,-0.5*sizeRect,-2*r],size=sizeRect*1.8)

oGround = mbs.AddObject(ObjectGround(referencePosition = [0,0,0], 
                           visualization = VObjectGround(graphicsData = [graphicsBackground])))


graphicsSphere = graphics.Sphere(point=[0,0,0], radius=r, color=graphics.color.steelblue, nTiles = 16)

nR0 = mbs.AddNode(Point2D(referenceCoordinates=[L,0]))
oR0 = mbs.AddObject(MassPoint2D(nodeNumber=nR0, mass=mass, visualization=VMassPoint2D(graphicsData=[graphicsSphere])))

mGround0 = mbs.AddMarker(MarkerBodyPosition(bodyNumber=oGround, localPosition = [0,0,0]))
mTip0 = mbs.AddMarker(MarkerNodePosition(nodeNumber=nR0))

if not withUserFunction: #with internal terms:
    oCD0 = mbs.AddObject(DistanceConstraint(markerNumbers=[mGround0, mTip0], distance=L))
else:
    #just for drawing, with inactive connector:
    mbs.AddObject(DistanceConstraint(markerNumbers=[mGround0, mTip0], distance=L, activeConnector=False))

    nGround = mbs.AddNode(NodePointGround())
    mCoordsGround = mbs.AddMarker(MarkerNodeCoordinates(nodeNumber=nGround))
    mCoords0 = mbs.AddMarker(MarkerNodeCoordinates(nodeNumber=nR0))
    
    #constraint user function:
    def UFconstraint(mbs, t, itemNumber, q, q_t,velocityLevel):
        #print("q=", q, ", q_t=", q_t)
        return [np.sqrt(q[0]**2 + q[1]**2) - L]

    #constraint jacobian user function:
    def UFjacobian(mbs, t, itemNumber, q, q_t,velocityLevel):
        #print("q=", q, ", q_t=", q_t)
        jac  = np.zeros((1,2))

        f = np.sqrt(q[0]**2 + q[1]**2)
        jac[0,0] = q[0]/f
        jac[0,1] = q[1]/f
        return jac
        
    mbs.AddObject(CoordinateVectorConstraint(markerNumbers=[mCoordsGround, mCoords0],
                                             scalingMarker0=np.zeros((1,2)), #needed to define number of algebraic equations; rows=nAE, cols=len(q) of mCoordsGround + mCoords0
                                             constraintUserFunction=UFconstraint,
                                             jacobianUserFunction=UFjacobian))
        
#
mbs.AddLoad(Force(markerNumber = mTip0, loadVector = [0, -mass*g, 0])) 

fileNameDouble = 'solution/coordVecConstraintRefDouble.txt'
fileNameSingle = 'solution/coordVecConstraintRefSingle.txt'
    
sPos0 = mbs.AddSensor(SensorNode(nodeNumber = nR0, storeInternal = True,
                                 #fileName=fileNameSingle, #single pendulum
                                 outputVariableType=exu.OutputVariableType.Position))


#for double pendulum, we add a second link
if doublePendulum:
    graphicsSphere = graphics.Sphere(point=[0,0,0], radius=r, color=graphics.color.red, nTiles = 16)
    nR1 = mbs.AddNode(Point2D(referenceCoordinates=[L*2,0]))
    oR1 = mbs.AddObject(MassPoint2D(nodeNumber=nR1, mass=mass, visualization=VMassPoint2D(graphicsData=[graphicsSphere])))
    
    mTip1 = mbs.AddMarker(MarkerNodePosition(nodeNumber=nR1))

    if not withUserFunction: #with internal terms:
        oCD1 = mbs.AddObject(DistanceConstraint(markerNumbers=[mTip0, mTip1], distance=L))
    else:
        #just for drawing, with inactive connector:
        mbs.AddObject(DistanceConstraint(markerNumbers=[mTip0, mTip1], distance=L, activeConnector=False))

        mCoords0 = mbs.AddMarker(MarkerNodeCoordinates(nodeNumber=nR0))
        mCoords1 = mbs.AddMarker(MarkerNodeCoordinates(nodeNumber=nR1))
 
        #constraint user function:
        def UFconstraint2(mbs, t, itemNumber, q, q_t,velocityLevel):
            #print("q=", q, ", q_t=", q_t)
            return [np.sqrt((q[2]-q[0])**2 + (q[3]-q[1])**2) - L]

        #constraint jacobian user function:
        def UFjacobian2(mbs, t, itemNumber, q, q_t,velocityLevel):
            #print("q=", q, ", q_t=", q_t)
            jac  = np.zeros((1,4))
            f = np.sqrt((q[2]-q[0])**2 + (q[3]-q[1])**2)
            jac[0,0] =-(q[2]-q[0])/f
            jac[0,1] =-(q[3]-q[1])/f
            jac[0,2] = (q[2]-q[0])/f
            jac[0,3] = (q[3]-q[1])/f
            return jac
            
        mbs.AddObject(CoordinateVectorConstraint(markerNumbers=[mCoords0, mCoords1],
                                                 scalingMarker0=np.zeros((1,2+2)), #needed to define number of algebraic equations; rows=nAE, cols=len(q) of mCoordsGround + mCoords0
                                                 constraintUserFunction=UFconstraint2,
                                                 jacobianUserFunction=UFjacobian2))

 
    #
    mbs.AddLoad(Force(markerNumber = mTip1, loadVector = [0, -mass*g, 0])) 
    
    sPos1 = mbs.AddSensor(SensorNode(nodeNumber = nR1, storeInternal = True,
                                     #fileName=fileNameDouble,
                                     outputVariableType=exu.OutputVariableType.Position))
    

mbs.Assemble()

simulationSettings = exu.SimulationSettings()
simulationSettings.timeIntegration.newton.useModifiedNewton = False #Just for the test; modified Newton is usually faster

# testIsActive = True
tEnd = 1
h = 1e-3
if not testIsActive:
    tEnd = 1
    simulationSettings.timeIntegration.realtime.active = True
    simulationSettings.timeIntegration.realtime.factor = 3
    
simulationSettings.timeIntegration.numberOfSteps = int(tEnd/h)
simulationSettings.timeIntegration.endTime = tEnd

#simulationSettings.solution.file.writePeriod = h
simulationSettings.timeIntegration.verboseMode = 1
#simulationSettings.solution.file.writePeriod = tEnd/steps

simulationSettings.timeIntegration.generalizedAlpha.spectralRadius = 0.8 #SHOULD work with 0.9 as well

SC.visualizationSettings.nodes.showBasis=True

if not testIsActive:
    SC.renderer.Start()
    SC.renderer.DoIdleTasks()

mbs.SolveDynamic(simulationSettings)

p0=mbs.GetObjectOutputBody(oR0, exu.OutputVariableType.Position, localPosition=[0,0,0])
exu.Print("p0=", list(p0))
u=sum(p0)

exu.Print('solution of coordinateVectorConstraint=',u)

exu.sys['testResult'] = u


if not testIsActive:
    SC.renderer.DoIdleTasks()
    SC.renderer.Stop() #safely close rendering window!
    
    if doublePendulum:
        mbs.PlotSensor([sPos0,sPos0,sPos1,sPos1], components=[0,1,0,1], closeAll=True)
    else:
        mbs.PlotSensor([sPos0,sPos0], components=[0,1], closeAll=True)
```