2D Collision not Working in Pychrono

[Siddharth L]

Hi All,

I'm with simple two level MBD Mechanism, In that 2D Collision is not working.
I not sure it due bug or, I'm not handling it properly. Kindly help me to resolve this issue.

Code :
```

import pychrono.core as chrono

import pychrono.irrlicht as chronoirr

# --- SYSTEM & MATERIALS ---

sys = chrono.ChSystemNSC()

mat = chrono.ChContactMaterialNSC()

sys.SetGravitationalAcceleration(chrono.ChVector3d(0, 0, 0))

# --- KEY REFERENCE POINTS (GENERIC NAMES) ---

# BODY 1 (main rotating body)

body1_pivot = chrono.ChVector3d(0, 13.692, 0)

body1_armA_end = chrono.ChVector3d(0, -13.692, 0)

body1_armB_end = chrono.ChVector3d(-12.74229169, -25.36073728, 0)

# BODY 2 (secondary body)

body2_pivot = chrono.ChVector3d(1.40186, -22.50787380, 0)

body2_armA_end = chrono.ChVector3d(-11.27933961, 11.62809944, 0)

body2_armB_end = chrono.ChVector3d(5.64177978, -19.92728796, 0)

# --- STATIC REFERENCE BODY ---

staticbody = chrono.ChBody()

staticbody.SetFixed(True)

staticbody.SetPos(chrono.ChVector3d(0, 0, 0))

sys.Add(staticbody)

# --- BODY 1 SETUP ---

body1_axis_sys = chrono.ChFramed(chrono.ChCoordsysd(body1_pivot, chrono.QUNIT))

body1 = chrono.ChBody()

body1.SetPos(body1_axis_sys.GetPos())

body1.SetFixed(False)

body1.SetMass(1)

body1.SetInertiaXX(chrono.ChVector3d(1e3, 1e3, 1e3))

body1.EnableCollision(True)

# Collision shape on BODY 1 (a small cylinder located at armB end)

body1_contact_cyl = chrono.ChCollisionShapeCylinder(mat, 0.81823296, 0.1)

body1.AddCollisionShape(body1_contact_cyl, chrono.ChFramed(body1_armB_end, chrono.QUNIT))

sys.Add(body1)

# Visual cylinder at the same place

body1_vis_cyl = chrono.ChVisualShapeCylinder(0.81823296, 0.1)

body1.AddVisualShape(body1_vis_cyl, chrono.ChFramed(body1_armB_end, chrono.QUNIT))

# Visualize BODY 1 arms (lines from pivot-local origin to endpoints)

local_body1_armA_end = chrono.ChVector3d(0, -13.692, 0)

local_body1_armB_end = chrono.ChVector3d(-12.74229169, -25.36073728, 0)

body1_armA_visual = chrono.ChVisualShapeLine()

body1_armA_visual.SetColor(chrono.ChColor(0.1, 0.9, 0.1))

body1_armA_visual.SetLineGeometry(chrono.ChLineSegment(chrono.ChVector3d(0, 0, 0), local_body1_armA_end))

body1_armB_visual = chrono.ChVisualShapeLine()

body1_armB_visual.SetColor(chrono.ChColor(0.1, 0.9, 0.1))

body1_armB_visual.SetLineGeometry(chrono.ChLineSegment(chrono.ChVector3d(0, 0, 0), local_body1_armB_end))

body1.AddVisualShape(body1_armA_visual)

body1.AddVisualShape(body1_armB_visual)

# Revolute joint for BODY 1 at body1_pivot

body1_revolute = chrono.ChLinkLockRevolute()

mframe = chrono.ChFramed(body1_pivot, chrono.QUNIT)

body1_revolute.Initialize(body1, staticbody, mframe)

sys.Add(body1_revolute)

min_angle = 0 * chrono.CH_DEG_TO_RAD

max_angle = 270 * chrono.CH_DEG_TO_RAD

body1_revolute.LimitRz().SetActive(True)

body1_revolute.LimitRz().SetMin(min_angle)

body1_revolute.LimitRz().SetMax(max_angle)

# Applied force/torque on BODY 1

body1_force = chrono.ChForce()

body1.AddForce(body1_force)

body1_force.SetMode(chrono.ChForce.FORCE)

body1_force.SetDir(chrono.ChVector3d(1, 0, 0))

body1_force.SetVrelpoint(body1_armA_end)

body1_force.SetMforce(5)  # magnitude

# --- BODY 2 SETUP ---

body2 = chrono.ChBody()

body2.SetPos(body2_pivot)  # Center of mass

body2.SetFixed(False)

body2.SetInertiaXX(chrono.ChVector3d(1e3, 1e3, 1e3))

body2.EnableCollision(True)

body2.SetMass(1)

sys.AddBody(body2)

# Revolute joint for BODY 2 at body2_pivot

body2_revolute = chrono.ChLinkLockRevolute()

mframe = chrono.ChFramed(body2_pivot, chrono.QUNIT)

body2_revolute.Initialize(staticbody, body2, mframe)

sys.Add(body2_revolute)

min_angle = 90 * chrono.CH_DEG_TO_RAD

max_angle = 180 * chrono.CH_DEG_TO_RAD

body2_revolute.LimitRz().SetActive(True)

body2_revolute.LimitRz().SetMin(min_angle)

body2_revolute.LimitRz().SetMax(max_angle)

# Contact material (for 2D arc + cylinder on body2)

contact_mat = chrono.ChContactMaterialNSC()

contact_mat.SetFriction(0.4)

contact_mat.SetRestitution(0.1)

# BODY 2: 2D arc shape (collision + visual)

center = chrono.ChCoordsysd(chrono.ChVector3d(0, 0, 0))

radius = 17

angle_start = 134.128 * chrono.CH_DEG_TO_RAD

angle_end = 142.532 * chrono.CH_DEG_TO_RAD

arc = chrono.ChLineArc(center, radius, angle_start, angle_end, True)

arc_shape = chrono.ChCollisionShapeArc2D(contact_mat, arc, radius)

body2.AddCollisionShape(arc_shape)

body2_arc_visual = chrono.ChVisualShapeLine()

body2_arc_visual.SetColor(chrono.ChColor(0.9, 0.1, 0.1))

body2_arc_visual.SetLineGeometry(arc)

body2.AddVisualShape(body2_arc_visual)

# BODY 2 arms (visual)

body2_armB_visual = chrono.ChVisualShapeLine()

body2_armB_visual.SetColor(chrono.ChColor(0.1, 0.9, 0.1))

body2_armB_visual.SetLineGeometry(chrono.ChLineSegment(chrono.ChVector3d(0, 0, 0), body2_armB_end))

body2.AddVisualShape(body2_armB_visual)

# BODY 2 cylinder contact + visual at armA end

body2_contact_cyl = chrono.ChCollisionShapeCylinder(mat, 0.8, 0.1)

body2.AddCollisionShape(body2_contact_cyl, chrono.ChFramed(body2_armA_end, chrono.QUNIT))

body2_vis_cyl = chrono.ChVisualShapeCylinder(0.81823296, 0.1)

body2.AddVisualShape(body2_vis_cyl, chrono.ChFramed(body2_armA_end, chrono.QUNIT))

# Applied force/torque on BODY 2

body2_force = chrono.ChForce()

body2.AddForce(body2_force)

body2_force.SetMode(chrono.ChForce.FORCE)

body2_force.SetDir(chrono.ChVector3d(1, 0, 0))

body2_force.SetVrelpoint(body2_armB_end)

body2_force.SetMforce(5)

# --- VISUALIZATION & SIMULATION LOOP ---

vis = chronoirr.ChVisualSystemIrrlicht()

vis.AttachSystem(sys)

vis.SetWindowSize(1024, 768)

vis.SetWindowTitle('Two-Body Mechanism ')

vis.Initialize()

# vis.SetBackgroundColor(chrono.ChColor(1, 1, 1))

vis.AddCamera(chrono.ChVector3d(0, 0, 60))

vis.AddTypicalLights()

vis.EnableAbsCoordsysDrawing(True)

vis.EnableLinkFrameDrawing(True)

while vis.Run():

    vis.BeginScene()

    vis.Render()

    vis.EndScene()

    sys.DoStepDynamics(1e-3)

```

[Radu Serban]

Siddharth,

 

You cannot mix 2D and 3D collision shapes.  If your mechanism is planar, all collision shapes must be 2D and lie in the same plane.

I pushed a Python demo illustrating the use of 2D collision shapes. See demo_MBS_collision_2d.py.

 

--Radu

 

P.S. Please post code with no background. The message below is difficult to read.

--
You received this message because you are subscribed to the Google Groups "ProjectChrono" group.
To unsubscribe from this group and stop receiving emails from it, send an email to projectchron...@googlegroups.com.
To view this discussion visit https://groups.google.com/d/msgid/projectchrono/f1593e24-73e1-474b-b250-853b68e2c7a6n%40googlegroups.com.

[Siddharth L]

Thank you, Radu.
Your example file helped me a lot in modeling my mechanism. However, the collision is still not working, and I’m not sure whether it’s a bug or if I’m not handling it correctly.
Here is my code: 
```
import pychrono as chrono
import pychrono.irrlicht as chronoirr
import math

FORCE = 10
MASS = 1
INERTIA = 1
SAFE_MARGIN = 0


sys = chrono.ChSystemNSC()
sys.SetCollisionSystemType(chrono.ChCollisionSystem.Type_BULLET)
# chrono.ChCollisionModel.SetDefaultSuggestedEnvelope(1)
# chrono.ChCollisionModel.SetDefaultSuggestedMargin(1)


contact_mat = chrono.ChContactMaterialNSC()

# Create the ground body
ground = chrono.ChBody()
ground.SetFixed(True)
sys.Add(ground)

# BODY 1
local_origin = chrono.ChVector3d(0, 0, 0)

body1_pivot = chrono.ChVector3d(0, 13.692, 0)
body1_armA_end = chrono.ChVector3d(0, -13.692, 0)
body1_armB_end = chrono.ChVector3d(-12.74229169, -25.36073728, 0)

Rr = 0.81823296

# BODY 2

body2_pivot = chrono.ChVector3d(1.40186, -22.50787380, 0)
body2_armA_end = chrono.ChVector3d(-11.27933961, 11.62809944, 0)
body2_armB_end = chrono.ChVector3d(5.64177978, -19.92728796, 0)

Rp = 17
body2_arc_start = 134.128 * chrono.CH_DEG_TO_RAD
body2_arc_end = 150.532 * chrono.CH_DEG_TO_RAD

# BODY 3
body3_pivot = chrono.ChVector3d(-2.22722111, -60.06300000, 0)
body3_armA_end = chrono.ChVector3d(5.98395158, 21.89565732, 0)
body3_armB_end = chrono.ChVector3d(-24.06887303, -12.10667160, 0)
Rl = 2

# --- BODY 1 SETUP ---
body1_axis_sys = chrono.ChFramed(chrono.ChCoordsysd(body1_pivot, chrono.QUNIT))

body1 = chrono.ChBody()
body1.SetPos(body1_axis_sys.GetPos())
body1.SetFixed(False)

body1.EnableCollision(True)
sys.Add(body1)

body1_path = chrono.ChLinePath()
body1_path.SetClosed(False)
body1_cir1 = chrono.ChLineArc(chrono.ChCoordsysd(body1_armB_end), Rr, 0, chrono.CH_2PI, True)
body1_path.AddSubLine(body1_cir1)

body1_coll = chrono.ChCollisionShapePath2D(contact_mat, body1_path)
body1.AddCollisionShape(body1_coll, chrono.ChFramed())
body1.GetCollisionModel().SetSafeMargin(SAFE_MARGIN)

body1_asset = chrono.ChVisualShapeLine()
body1_asset.SetLineGeometry(body1_path)
body1.AddVisualShape(body1_asset)

body1_arm1 = chrono.ChLineSegment(local_origin, body1_armA_end)
body1_arm2 = chrono.ChLineSegment(local_origin, body1_armB_end)


body1_arm_coll1 = chrono.ChCollisionShapeSegment2D(contact_mat, body1_arm1, 0.2)
body1_arm_coll2 = chrono.ChCollisionShapeSegment2D(contact_mat, body1_arm2, 0.2)

body1_arm1_vis = chrono.ChVisualShapeLine()
body1_arm1_vis.SetLineGeometry(body1_arm1)
body1.AddVisualShape(body1_arm1_vis)

body1_arm2_vis = chrono.ChVisualShapeLine()
body1_arm2_vis.SetLineGeometry(body1_arm2)
body1.AddVisualShape(body1_arm2_vis)

body1_revolute = chrono.ChLinkLockRevolute()
mframe = chrono.ChFramed(body1_axis_sys.GetPos(), chrono.QUNIT)
body1_revolute.Initialize(body1, ground, mframe)

min_angle = 0 * chrono.CH_DEG_TO_RAD
max_angle = 270 * chrono.CH_DEG_TO_RAD
body1_revolute.LimitRz().SetActive(True)
body1_revolute.LimitRz().SetMin(min_angle)
body1_revolute.LimitRz().SetMax(max_angle)

sys.Add(body1_revolute)


force_on_body1 = chrono.ChForce()
body1.AddForce(force_on_body1)
force_on_body1.SetMode(chrono.ChForce.FORCE)
force_on_body1.SetDir(chrono.ChVector3d(1, 0, 0))
force_on_body1.SetVrelpoint(body1_armA_end)
force_on_body1.SetMforce(FORCE)

# -------------- BODY 2 -----------------
body2_axis_sys = chrono.ChFramed(chrono.ChCoordsysd(body2_pivot, chrono.QUNIT))

body2 = chrono.ChBody()
body2.SetPos(body2_axis_sys.GetPos())
body2.SetFixed(False)
body2.EnableCollision(True)
sys.Add(body2)

body2_path1 = chrono.ChLinePath()
body2_path1.SetClosed(False)
body2_arc1 = chrono.ChLineArc(chrono.ChCoordsysd(local_origin), Rp, body2_arc_start, body2_arc_end, True)
body2_path1.AddSubLine(body2_arc1)

body2_coll1 = chrono.ChCollisionShapePath2D(contact_mat, body2_path1)
body2.AddCollisionShape(body2_coll1, chrono.ChFramed())
body2.GetCollisionModel().SetSafeMargin(SAFE_MARGIN)

body2_asset1 = chrono.ChVisualShapeLine()
body2_asset1.SetLineGeometry(body2_path1)
body2.AddVisualShape(body2_asset1)

body2_path2 = chrono.ChLinePath()
body2_path2.SetClosed(False)
body2_arc2 = chrono.ChLineArc(chrono.ChCoordsysd(body2_armA_end), 0.8, 0, chrono.CH_2PI, True)
body2_path2.AddSubLine(body2_arc2)

body2_coll2 = chrono.ChCollisionShapePath2D(contact_mat, body2_path2)
body2.AddCollisionShape(body2_coll2, chrono.ChFramed())
body2.GetCollisionModel().SetSafeMargin(SAFE_MARGIN)

body2_asset2 = chrono.ChVisualShapeLine()
body2_asset2.SetLineGeometry(body2_path2)
body2.AddVisualShape(body2_asset2)

body2_arm2 = chrono.ChLineSegment(local_origin, body2_armB_end)


body2_arm_coll = chrono.ChCollisionShapeSegment2D(contact_mat, body2_arm2, 0.2)

body2_arm2_vis = chrono.ChVisualShapeLine()
body2_arm2_vis.SetLineGeometry(body2_arm2)
body2.AddVisualShape(body2_arm2_vis)

body2_revolute = chrono.ChLinkLockRevolute()
mframe = chrono.ChFramed(body2_axis_sys.GetPos(), chrono.QUNIT)
body2_revolute.Initialize(body2, ground, mframe)
sys.Add(body2_revolute)

# -------------- BODY 3 -----------------
body3_axis_sys = chrono.ChFramed(chrono.ChCoordsysd(body3_pivot, chrono.QUNIT))

body3 = chrono.ChBody()
body3.SetPos(body3_axis_sys.GetPos())
body3.SetFixed(False)

body3.SetAngVelLocal(chrono.ChVector3d(0, 0, -50))
body3.EnableCollision(True)
sys.Add(body3)

body3_path = chrono.ChLinePath()
body3_path.SetClosed(False)
body3_cir1 = chrono.ChLineArc(chrono.ChCoordsysd(body3_armA_end), Rl, 0, chrono.CH_2PI, True)
body3_path.AddSubLine(body3_cir1)

body3_coll = chrono.ChCollisionShapePath2D(contact_mat, body3_path)
body3.AddCollisionShape(body3_coll, chrono.ChFramed())
body3.GetCollisionModel().SetSafeMargin(SAFE_MARGIN)

body3_asset = chrono.ChVisualShapeLine()
body3_asset.SetLineGeometry(body3_path)
body3.AddVisualShape(body3_asset)

body3_arm1 = chrono.ChLineSegment(local_origin, body3_armA_end)
body3_arm2 = chrono.ChLineSegment(local_origin, body3_armB_end)


body3_arm_coll1 = chrono.ChCollisionShapeSegment2D(contact_mat, body3_arm1, 0.2)
body3_arm_coll2 = chrono.ChCollisionShapeSegment2D(contact_mat, body3_arm2, 0.2)

body3_arm1_vis = chrono.ChVisualShapeLine()
body3_arm1_vis.SetLineGeometry(body3_arm1)
body3.AddVisualShape(body3_arm1_vis)

body3_arm2_vis = chrono.ChVisualShapeLine()
body3_arm2_vis.SetLineGeometry(body3_arm2)
body3.AddVisualShape(body3_arm2_vis)

body3_revolute = chrono.ChLinkLockRevolute()
mframe = chrono.ChFramed(body3_axis_sys.GetPos(), chrono.QUNIT)
body3_revolute.Initialize(body3, ground, mframe)
sys.Add(body3_revolute)

# -------------- Visualization -----------------

vis = chronoirr.ChVisualSystemIrrlicht()
vis.AttachSystem(sys)
vis.SetWindowSize(1024, 768)

vis.SetWindowTitle("Test")
vis.SetBackgroundColor(chrono.ChColor(0.1, 0.2, 0.3))
vis.Initialize()
vis.AddCamera(chrono.ChVector3d(0, 0, 80))
vis.AddTypicalLights()
vis.EnableAbsCoordsysDrawing(True)
vis.EnableBodyFrameDrawing(True)
vis.EnableFullscreen(True)

# Simulation loop

while vis.Run():
    vis.BeginScene()
    vis.Render()
    vis.EndScene()
    sys.DoStepDynamics(1e-3)

```

[Radu Serban]

Siddharth,

 

I do not have time to debug your code. But maybe someone else on the forum could help you with that.

Having said that, my suggestion is to start simple: create only the first 2 bodies, with only the necessary collision geometry (and a corresponding visualization so that you can see what is happening, but no need to add more visual shapes that have no collision counterpart).  Once you get this first mechanism working as expected, add another body, and so on.

 

--Radu

To view this discussion visit https://groups.google.com/d/msgid/projectchrono/414a9df1-4686-4785-8978-9f1b9b02c627n%40googlegroups.com.

[Siddharth L]

Hi Radu,

I really want to thank you for your time and support.
I figured out the issue — the arc direction was the problem.
I had created the arc in the counter-clockwise direction by providing the angle from 0 to 2Φ with CCW=True. Later, I passed only the CoordSys and Radius.

Updated Code:

```
arc_circle = chrono.ChLineArc(chrono.ChCoordsysd(body1_armB_end), Rr)
for Arc used CW direction

​

``` 
I have a question about units.
Since PyChrono is unitless, my geometry is in mm, but when I switch to m, the values are too small and visualization becomes unstable.

Why is it necessary to use meters if I want consistent units (Force in N, Mass in kg, Velocity in m/s, Inertia in kg·m²)?
If I keep everything in mm and scale other quantities, simulation becomes unstable. What’s the proper way to handle this?

Also, what’s the best method to measure the force at a moving arm’s endpoint toward a fixed world point, and the moment at its pivot?
Any example or recommended approach would be helpful.

Thanks, 
Siddharth

[Radu Serban]

Hi Siddharth,

 

I am not sure I understand some of the issues you bring up.

1. What do you mean by “visualization becomes unstable”?  By the way, are you using Irrlicht or VSG?
2. Chrono is unit-agnostic. But that assumes that you use a consistent set of units. If you see issues with the simulation, this is a sign that you have not properly scaled everything.
3. What do you mean by “force at a moving arm’s endpoint”?  Is this a contact force? A reaction force? Something else?

To view this discussion visit https://groups.google.com/d/msgid/projectchrono/4e2b56c2-d50d-4c8d-b6c5-6ac8f6dbd57fn%40googlegroups.com.