# yade-users team mailing list archive

## Re: [Question #698572]: Is it possible to constrain the motion of a body along a circle

Rohit John is still having a problem:
Dear Jan,

Thanks for clearing my doubt and your idea. I figured out a method using
Lagrangian Dynamics and Lagrangian multipliers. The Idea is to write the
Lagrangian in the Cartesian coordinates and constrain the motion using a
Lagrangian Multiplier [1] . This will result in the Newtons equation
with and additional variable and one extra equation which defines the
constrain. Solving these equations will yield a force, that constrains
the body to a circle. But solving this is expensive as one of the
equation is non linear. I have posted a derivation here [2]. The
constraining force is given by [2*x*labmda, 2*y*labmda, 2*z*labmda]

This method seems to be very slow. I think its because it involves
solving a system of equations containing one non linear equation. Please
let me know if there is a way to speed this up.

Kind regards,
Rohit K. John

[2] https://imgur.com/a/1l1KjOs

# ------------------------------------------------------------------------------------------------------------------------------ Python
from math import *
from scipy.optimize import fsolve
import math

# ---------------------------------------------------------------------------------- bodies
sph1 = sphere((0, 0, 2),  .5)
sph2 = sphere(center=(0, 0, 0), radius=.5, fixed=True)
sph3 = sphere((0, -2, 2), .5)

sph1_id = O.bodies.append(sph1)
sph2_id = O.bodies.append(sph2)
sph3_id = O.bodies.append(sph3)

# # ---------------------------------------------------------------- Initialising
O.bodies[sph3_id].state.vel = [0,1,0]
g        = Vector3([0, 0, 0])
stiffnes = 1e7
length   = 2
lamda    = 0

# ---------------------------------------------------------------------------------- engines
O.engines = [
ForceResetter(),
InsertionSortCollider([Bo1_Sphere_Aabb()]),
InteractionLoop(
[Ig2_Sphere_Sphere_ScGeom()],  # collision geometry
[Ip2_FrictMat_FrictMat_FrictPhys()],  # collision "physics"
[Law2_ScGeom_FrictPhys_CundallStrack()]  # contact law -- apply forces
),
PyRunner(command = "constrainingForceEngine()", iterPeriod = 1),
NewtonIntegrator(gravity=g, damping=0.0),
]

def constrainingForceEngine():
"""
Calculates the constraining force required keep the particle in a circle
"""
# initializing
global lamda
m  = O.bodies[sph1_id].state.mass
dt = O.dt

x0,  y0,  z0   = O.bodies[sph1_id].state.pos
vx0, vy0, vz0  = O.bodies[sph1_id].state.vel
Fx0, Fy0, Fz0  = O.forces.f(sph1_id) +  m*g

eq = EOM_maker(x0, y0, z0, vx0 ,vy0, vz0, dt, m, Fx0, Fy0, Fz0, length)

# Solving the equation
x, y, z, vx, vy, vz, lamda =  fsolve(
eq,
(x0,  y0,  z0, vx0, vy0, vz0, lamda)
)

# Assiging the force
Fx = 2 * lamda * x
Fy = 2 * lamda * y
Fz = 2 * lamda * z
force = Vector3([Fx, Fy, Fz])
O.forces.addF(id = sph1_id, f = force)

def EOM_maker(x0, y0, z0, vx0 ,vy0, vz0, dt, m, F0x, F0y, F0z, l):
"""
This function returns a function which is the equation of motion with an additional lagrangian
multiplier term (2*lamda*x) and the equation of constrain
"""
def EOM(p):
x, y, z, vx, vy, vz, lamda = p
return (
vx - vx0 - dt * (F0x + 2*lamda*x)/m,
x  - x0  - vx*dt,
vy - vy0 - dt * (F0y + 2*lamda*y)/m,
y  - y0  - vy*dt,
vz - vz0 - dt * (F0z + 2*lamda*z)/m,
z  - z0  - vz*dt,
x**2 + y**2 + z**2 - l**2
)

return EOM
# ---------------------------------------------------------------------------------- sim setup
O.dt = .5e-2 * PWaveTimeStep()
O.saveTmp()

--