# Is it possible to constrain the motion of a body along a circle

Hello,

I want to constrain the motion of a body to a circle. The answer given in [1], recommends using a penalty force to constrain the motion of the body. If large forces act on the body, I suppose this method will not work unless the stiffness of the penalty force is increased. But I suppose this increase would require smaller dt to be stable. So is there any other way to constrain the motion of a body along a circle, one which does not lower the dt?

Kind regards,
Rohit K. John

## Question information

Language:
English Edit question
Status:
For:
Assignee:
No assignee Edit question
Last query:
 Revision history for this message Jan Stránský (honzik) said on 2021-08-31: #1

Hello,
please provide more information about your situation. Mainly if the constrained body has blocked DOFs (its motion is prescribed) or not (its motion is driven by collisions with other particles).
For the case of blocked DOFs, e.g. RotationEngine can be used.
For the "free" case, I think it still should be feasible to make it work easily.
Cheers
Jan

 Revision history for this message Rohit John (rohitkjohn) said on 2021-09-01: #2

Hello Jan,

I am trying to simulate a rigid box that is constrained to rotate about an axis that does pass through its centre of mass. The box is modelled used pFacets and all the pFacets are clumped together. This box is supposed to rotate freely and collide with another object, which is a brush modelled using gridConnections.

I tried blocking the DOF of the clump. This constrains the box to rotate about an axis passing through its centre of mass. I require it to rotate about an axis that is eccentric, one that does not pass through the centre of mass. From what I understand, the rotationEngine forces the body to rotate about a given axis at a given angular velocity. I want the body to rotate freely and react to collisions. Is that possible with the rotationEngine?

Thanks and Kind regards,
Rohit K. John

 Revision history for this message Karol Brzezinski (kbrzezinski) said on 2021-09-01: #3

Hi John,

aren't those two statements opposite:

>I am trying to simulate a rigid box that is constrained to rotate about an axis that does pass through its centre of mass.

>I require it to rotate about an axis that is eccentric, one that does not pass through the centre of mass.

I think that is a good time to ask about MWE [1]

Best wishes,
Karol

 Revision history for this message Rohit John (rohitkjohn) said on 2021-09-02: #4

Dear Karol,

> aren't those two statements opposite:
- You are correct, I made a mistake. It is supposed to be
"I am trying to simulate a rigid box that is constrained to rotate about an axis that DOES NOT PASS through its centre of mass. "
If I wanted the axis to pass through the center of mass, then using the blockedDOF variable of the body is sufficient.

> I think that is a good time to ask about MWE
- From what I understand a MWE is required when there is an error in the code. I am not facing an error but require guidance on how to constrain motion. Below I have given an example of what I require. I am simplified the problem. Instead of a box, I have used a clump made of two spheres. This clump is free to rotate about an axis passing through one of the spheres (NOT THE CENTRE OF MASS of the clump). I used a penalty force method discussed in [1] to constrain the motion of the clump. I want to know if there are other ways to achieve this.

Kind regards,
Rohit K. John

from math import *

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

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

g = Vector3([0,0,-10])
stiffnes = 1e7

# ---------------------------------------------------------------- clump
clump_id = O.bodies.clump([sph1_id, sph2_id])

# ---------------------------------------------------------------------------------- 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 = "eccentricAxisEngine()", iterPeriod = 1),
NewtonIntegrator(gravity=g, damping=0.0),
]

def eccentricAxisEngine():
pos_0 = O.bodies[sph2_id].state.pos
force = -stiffnes*pos_0
O.forces.addF(id = sph2_id, f = force)

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

 Revision history for this message Jan Stránský (honzik) said on 2021-09-03: #5

Hello,

> From what I understand, the rotationEngine forces the body to rotate about a given axis at a given angular velocity. I want the body to rotate freely and react to collisions. Is that possible with the rotationEngine?

Exactly. RotationEngine is for prescribed motion (given, although possibly changing, but given velocity), not usable for "free" motion.

> If large forces act on the body, I suppose this method will not work unless the stiffness of the penalty force is increased. But I suppose this increase would require smaller dt to be stable.

maybe you would need to decrease dt. But maybe not :-) An analysis or at least a trial-and-error evaluation would be needed to tell the relation of this "motion constraint" dt and the "normal collision dt"

> So is there any other way to constrain the motion of a body along a circle, one which does not lower the dt?

I would say yes. You can modify eccentricAxisEngine like this:
- after InteractionLoop, you know the force and torque on your body
- you know position and velocities, you are able to compute the new position (that would be computed by NewtonIntegrator)
- determine actual new position = closest point from the "potential new position" on the desired circle (or surface in a general case)
- modify velocities such that the true NewtonIntegrator results in this desired position on the circle

Just an idea, not sure if it works at all :-D (should) and not sure how difficult would be the "modified velocity" computation.
Let us know if you go this way and if you were successful. I am curious, but currently do not have time to play with it myself.

Cheers
Jan

 Revision history for this message Rohit John (rohitkjohn) said on 2021-09-03: #6

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

# ------------------------------------------------------------------------------------------------------------------------------ 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()

 Revision history for this message Karol Brzezinski (kbrzezinski) said on 2021-09-04: #7

Hi,

I came up with a simple idea, that could work. It doesn't require any optimization tools, so theoretically should be faster. Unfortunately, I my implementation was quite slow and still required regularization such ass your eccentricAxisEngine(). Maybe it was due to the numerical issues and maybe just erroneous code. Thus, I will not share the code because I don't want to suggest wrong solutions (unless you want the code), but I will share the idea behind this.

I limited the problem to the 2D space.
1) The rotation center is in point (0,0,0) and the center of mass travels along circle of radius (R). So the angular acceleration (omega") is related with linear acceleration in tangential direction (a") by kinematics: a" = R*omega". By knowing mass and inertia of the body this relations can be expressed in terms of unbalanced force (f_u) and unbalanced torque (t_u).
2) First I read unbalanced force from the forceContainer. I add the "force equivalent of gravity", and than I leave only component in the direction tangential to the circle (for the current position). Based on this and eq. from point (1), I can compute the desired (target) torque.
3) The force in point (2) is almost the target force, I need to add the component "balancing eccentric force", based on the actual velocity and given radius.
4) Knowing my target torque and target force I modify existing unbalanced forces by adding corrections.

Eventually I got confused by inertia convention in clumps. I am suspecting a bug, but maybe I should start another topic for that. Please note that your very first solution works like a charm if you stay in one plane (e.g. y-z).

It can be visualized by modifying fragment of your code:
######################
# ---------------------------------------------------------------------------------- bodies
sph1 = sphere((0, 2, 0), .5)
sph2 = sphere(center=(0, 0, 0), radius=.5)

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

g = Vector3([0,0,-10])
stiffnes = 1e7

# ---------------------------------------------------------------- clump
clump_id = O.bodies.clump([sph1_id, sph2_id])

####### additional sphere to collide with
sph3 = sphere((0, -2.25, 3), .5)
sph3_id = O.bodies.append(sph3)
#######################

Good luck!
Karol

 Revision history for this message Jan Stránský (honzik) said on 2021-09-05: #8

My "velocity" contribution (I could not resist and tried the "velocity" approach :-)

Recalling the idea:
- after InteractionLoop before NewtonIntegrator, forces (i.e. acceleration) of bodies are known
- pre-compute positions, that NewtonIntegrator would compute
- constrain this pre-computed position
- "trick" the velocity such that the actual NewtonIntegrator computes the desired constrained position

For linear components, first velocity is updated and then position is updated:
vel_next = vel_prev + accel * dt
pos_next = pos_prev + vel_next * dt = pos_prev + (vel_prev + accel*dt)*dt =
= pos_prev + vel_prev*dt + accel*dt*dt

Knowing pos_next, constrain it: pos_next -> pos_next_constrained (e.g. closest point projection)
Rewrite the pos_next equation for pos_next_constrained:
pos_next_constrained =pos_prev + vel_prev*dt + accel*dt*dt
Solve for vel_prev:
vel_prev = (pos_next_constrained - pos_prev) / dt - accel*dt

In your specific case, the orientation is directly related to position.
But I think that for a general case, it should be possible to "trick" the angular velocity similarly

Code
====

Approach 1, linear velocity only (results in pedal-like motion):
###
sph1 = sphere((0, -2, 2), .5)
sph2 = sphere((0, 0, 0), .5)
clump_id,(sph1_id,sph2_id) = O.bodies.appendClumped((sph1,sph2))
clump = O.bodies[clump_id]
O.engines = [
ForceResetter(),
InsertionSortCollider([Bo1_Sphere_Aabb()]),
InteractionLoop(
[Ig2_Sphere_Sphere_ScGeom()],
[Ip2_FrictMat_FrictMat_FrictPhys()],
[Law2_ScGeom_FrictPhys_CundallStrack()]
),
PyRunner(command = "eccentricAxisEngine()", iterPeriod = 1),
NewtonIntegrator(gravity=(0,0,-10), damping=0, label="newton"),
]
O.dt = .5e-2 * PWaveTimeStep()

def constraintClosesPoint(pos):
delta = pos - center
direction = delta.normalized()
return center + direction * radius
def eccentricAxisEngine():
dt = O.dt
s = clump.state
f = O.forces.f(clump_id)
a = f / s.mass + newton.gravity
pos1 = s.pos + (s.vel+a*dt)*dt
pos0 = constraintClosesPoint(pos1)
s.vel = (pos0 - s.pos) / dt - a*dt
###

Approach 2, linear + angular velocity (perfect results):
###
sph1 = sphere((0, -2, 2), .5)
sph2 = sphere((0, 0, 0), .5)
clump_id,(sph1_id,sph2_id) = O.bodies.appendClumped((sph1,sph2))
clump = O.bodies[clump_id]
O.engines = [
ForceResetter(),
InsertionSortCollider([Bo1_Sphere_Aabb()]),
InteractionLoop(
[Ig2_Sphere_Sphere_ScGeom()],
[Ip2_FrictMat_FrictMat_FrictPhys()],
[Law2_ScGeom_FrictPhys_CundallStrack()]
),
PyRunner(command = "eccentricAxisEngine()", iterPeriod = 1),
NewtonIntegrator(gravity=(0,0,-10), damping=0, label="newton"),
]
O.dt = .5e-2 * PWaveTimeStep()

clump.state.refOri = clump.state.ori # not done automatically
clump.state.blockedDOFs = "XYZ" # to force "simple" angular integration

def constraintClosesPoint(pos):
delta = pos - center
direction = delta.normalized()
pos = center + direction * radius
angle = atan2(direction[2],direction[1])
ori = Quaternion()
ori.setFromTwoVectors(Vector3.UnitX,direction)
return pos,ori
def eccentricAxisEngine():
dt = O.dt
s = clump.state
f = O.forces.f(clump_id)
a = f / s.mass + newton.gravity
pos1 = s.pos + (s.vel+a*dt)*dt
pos0,ori0 = constraintClosesPoint(pos1)
s.vel = (pos0 - s.pos) / dt - a*dt
#
v1 = s.ori * Vector3.UnitX
v2 = ori0 * Vector3.UnitX
ang = v1.cross(v2)
ang = ang / O.dt
s.angVel = ang
###

Cheers
Jan

 Revision history for this message Rohit John (rohitkjohn) said on 2021-09-06: #9

Dear Karol and Jan,

Thanks for taking time from your work to come up with really creative solutions to my problem. I will test the solutions and let you know what I find.

Kind regards,
Rohit K. John