CadQuery / cadquery

A python parametric CAD scripting framework based on OCCT
https://cadquery.readthedocs.io
Other
3.21k stars 291 forks source link

Body rotations from a repeatedly solved assembly seem to be off by factor 1.29 #1651

Open unrealBob opened 2 months ago

unrealBob commented 2 months ago

I recently got into creating animations with CadQuery and jupyter_cadquery and wanted to animate more complex constrained assemblies (planar linkages). My approach currently is just changing the param of a fixed rotation constraint for given timesteps, solving the assembly and querying the location of the bodies. The locations can then be used to create an animation.

I noticed something strange when querying the rotation of the solved assembly. When solving the first time, the rotation is as expected but after the second solve, the angle of the body is consistently 1.29 times higher than the set angle, which results in the animation "overshooting".

I altered the example from the assembly documentation. Can this be reproduced or am I missing something obvious?

from math import pi
import cadquery as cq

b1 = cq.Workplane().box(1, 1, 1)
b2 = cq.Workplane().rect(0.1, 0.1).extrude(1)

assy = (
    cq.Assembly()
    .add(b1, name="b1")
    .add(b2, loc=cq.Location((0, 0, 4)), name="b2", color=cq.Color("red"))
)
assy.name = "assy"

# fix the position of b1
assy.constrain("b1", "Fixed")
# fix b2 bottom face position (but not rotation)
assy.constrain("b2@faces@<Z", "FixedPoint", (0, 0, 0.5))
# fix b2 rotational degrees of freedom too
assy.constrain("b2", "FixedRotation", (45, 0, 0))
assy.solve()
print("initial solve: ", round(assy.objects.get("b2").loc.toTuple()[1][0] * 180/pi, 5))

angles = [0, 45, 0, -90, 0]
rotX = []
for angle in angles:
    # changing the angle and solving assembly 
    assy.constraints[-1].param = (angle, 0, 0)
    assy.solve()

    # query the location ((tx, ty, tz), (rx, ry, rz)) of b2
    b2_loc = assy.objects.get("b2").loc.toTuple()
    # get rx value, round and put it in list as degree
    rotX.append(round(b2_loc[1][0] * 180/pi, 1))

    print(f"set Angle:\t {assy.constraints[-1].param[0]}")
    print(f"solved angle:\t {rotX[-1]}")

"""animation part, for the sake of completeness
from jupyter_cadquery.animation import Animation
from jupyter_cadquery.viewer.client import show
show(assy)
time = [0, 1, 2, 3, 4]
animation = Animation()
animation.add_track(f"/{assy.name}/b2", "rx", time, rotX)
animation.animate(2)
"""
adam-urbanczyk commented 2 months ago

.param is in radians AFAICT. Probably it should be marked as private (_parmas).

angles = [0, 45, 0, -90, 0]
rotX = []
for angle in angles:
    # changing the angle and solving assembly 
    assy.constraints[-1].param = (radians(angle), 0, 0)
    assy.solve()
unrealBob commented 2 months ago

Thank you very much for the fast response!

The issue is not with the radians or degree conversion. In the example from the documentation its written in degree:

# fix b2 rotational degrees of freedom too
assy.constrain("b2", "FixedRotation", (45, 0, 45))

When querying the body rotation within a solved assembly however, it is in radians.

The issue is that when solving the assembly for the first time, the returned body rotation is correct, when solving it the second (or any other) time with a changed param value, it is off by 1.29 (obviously regardless of rad or deg), but consistently.

adam-urbanczyk commented 2 months ago

What I can see is that the last solve in your example is not fully converged (assy.solve(5) to get some diagnostics). If you rerun the last solve, it does return the correct value. I do not get the off by 1.29 statement.

lorenzncode commented 2 months ago

Is it an issue with the starting point? I can reproduce the not fully converged result with this example:


import cadquery as cq

b1 = cq.Workplane().box(1, 1, 1)
b2 = cq.Workplane().rect(0.1, 0.1).extrude(1)

# bad starting point
starting_rot = (-90, 0, 0)

# optimal solution found
# starting_rot = (10, 0, 0)

assy = (
    cq.Assembly()
    .add(b1, name="b1")
    .add(
        b2,
        loc=cq.Location((0, 0, 0), starting_rot),
        name="b2",
        color=cq.Color("red"),
    )
)

assy.constrain("b1", "Fixed")
# fix b2 bottom face position (but not rotation)
assy.constrain("b2@faces@<Z", "FixedPoint", (0, 0, 0.5))
# fix b2 rotational degrees of freedom too
assy.constrain("b2", "FixedRotation", (0, 0, 0))

assy.solve(5)
print(f'after solve: = {assy.objects.get("b2").loc.toTuple()}')
unrealBob commented 2 months ago

Okay I think I know where my issue comes from. Lets take a look at the following lines:

# set the initial rotation to 45 degree 
assy.constrain("b2", "FixedRotation", param = (45, 0, 0))
assy.solve()
print("initial solve: ", round(assy.objects.get("b2").loc.toTuple()[1][0] * 180/pi, 5))

# changing the desired angle of the body to 90 degree
assy.constraints[-1].param = (pi/2, 0, 0)
assy.solve()
print("second solve: ", round(assy.objects.get("b2").loc.toTuple()[1][0] * 180/pi, 5))

When creating the FixedRotation constraint and defining param = (45,0,0), the angle after solving is 45 degree as expected. When updating the constraint with assy.constraints[-1].param = (pi/2, 0, 0) the angle indeed needs to be set in rad. Now I get the expected angle of 90 degrees. This really confused me.

Thank you very much @adam-urbanczyk. Your first answer was correct.

TLDR: