You can not select more than 25 topics
Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
203 lines
6.0 KiB
203 lines
6.0 KiB
import pymesh
|
|
import json
|
|
import pathlib
|
|
import copy
|
|
import math
|
|
import itertools
|
|
|
|
import numpy
|
|
from scipy.spatial.transform import Rotation
|
|
|
|
import context
|
|
|
|
from fixture_utils import save_fixture, get_fixture_dir_path, get_meshes_dir_path
|
|
|
|
scale = 1
|
|
|
|
barring = {
|
|
"mesh": "507-movements/227-chain-pully/barring.obj",
|
|
"rotation": [90, 0, 0],
|
|
"scale": scale
|
|
}
|
|
|
|
pin = {
|
|
"mesh": "507-movements/227-chain-pully/pin.obj",
|
|
"rotation": [90, 0, 0],
|
|
"scale": scale
|
|
}
|
|
|
|
link = {
|
|
"mesh": "507-movements/227-chain-pully/link.obj",
|
|
"rotation": [90, 0, 0],
|
|
"scale": scale
|
|
}
|
|
link_hole_center = 2.45905
|
|
link_width = 2 * link_hole_center
|
|
link_vertical_offsets = [0.763387, 0.940965]
|
|
|
|
|
|
def circle_points(num_links, angle_offset=0, radius_offset=0):
|
|
angles = numpy.linspace(angle_offset, 2 * numpy.pi + angle_offset,
|
|
num=num_links, endpoint=False).reshape(-1, 1)
|
|
radius = link_width / (2 * numpy.sin(numpy.pi / num_links)) + radius_offset
|
|
x = radius * numpy.cos(angles)
|
|
y = radius * numpy.sin(angles)
|
|
z = numpy.zeros(angles.shape)
|
|
return numpy.hstack([x, y, z])
|
|
|
|
|
|
def line_points(start, dir, num_links):
|
|
points = numpy.empty((num_links + 1, 3))
|
|
points[0] = start
|
|
dir /= numpy.linalg.norm(dir)
|
|
for i in range(num_links):
|
|
points[i + 1] = points[i] + link_width * dir
|
|
return points
|
|
|
|
|
|
def export_polyline(points, offset=0, loops=True):
|
|
for point in points:
|
|
print("v {:g} {:g} {:g}".format(*point))
|
|
for i in range(len(points) - 1):
|
|
print(f"l {i + 1 + offset:d} {i + 2 + offset:d}")
|
|
if loops:
|
|
print(f"l {len(points) + offset:d} {1 + offset:d}")
|
|
|
|
|
|
def polyline_to_chain(points):
|
|
"""Assumes the lines are the links length"""
|
|
assert((points[0] != points[-1]).any()) # no loop
|
|
chain = []
|
|
num_points = points.shape[0]
|
|
assert(num_points % 2 == 0)
|
|
for i in range(num_points):
|
|
pi0 = points[i]
|
|
pi1 = points[(i + 1) % num_points]
|
|
|
|
chain.append(copy.deepcopy(link))
|
|
chain[-1]["position"] = (scale * (pi1 + pi0) / 2).tolist()
|
|
chain[-1]["position"][2] = scale * link_vertical_offsets[i % 2]
|
|
chain[-1]["rotation"][2] = numpy.arctan2(
|
|
*(pi1 - pi0)[:2][::-1]) * 180 / numpy.pi
|
|
|
|
chain.append(copy.deepcopy(chain[-1]))
|
|
chain[-1]["position"][2] *= -1
|
|
|
|
chain.append(copy.deepcopy(barring))
|
|
chain[-1]["position"] = (scale * pi0).tolist()
|
|
chain.append(copy.deepcopy(pin))
|
|
chain[-1]["position"] = (scale * pi1).tolist()
|
|
return chain
|
|
|
|
|
|
def generate_sprocket(num_links):
|
|
angles = numpy.linspace(0, 2 * numpy.pi, num=num_links,
|
|
endpoint=False).reshape(-1, 1)
|
|
angle_offset = (angles[0] + angles[1]) / 2
|
|
points = circle_points(num_links, angle_offset, radius_offset=-1.5)
|
|
|
|
spike = pymesh.load_mesh(str(get_meshes_dir_path() /
|
|
"507-movements/227-chain-pully/spike.obj"))
|
|
spikes = []
|
|
for i in range(num_links):
|
|
R = Rotation.from_euler(
|
|
'xyz', [90, 0, (i + 0.5) * 360 / num_links], degrees=True)
|
|
R = R.as_matrix()
|
|
spikes.append(
|
|
pymesh.form_mesh(spike.vertices @ R.T + points[i], spike.faces)
|
|
)
|
|
|
|
points = circle_points(num_links, 0, radius_offset=-1)
|
|
x = points[:, 0].reshape(-1, 1)
|
|
y = points[:, 1].reshape(-1, 1)
|
|
points = numpy.vstack([numpy.hstack([x, y, numpy.full(angles.shape, spike.vertices[:, 1].min())]),
|
|
numpy.hstack([x, y, numpy.full(angles.shape, spike.vertices[:, 1].max())])])
|
|
|
|
sprocket = pymesh.convex_hull(
|
|
pymesh.form_mesh(points, numpy.empty((0, 3))))
|
|
|
|
print("Union of spikes")
|
|
for spike in spikes:
|
|
sprocket = pymesh.boolean(sprocket, spike, operation="union")
|
|
print("Done")
|
|
|
|
pymesh.save_mesh(str(get_meshes_dir_path() /
|
|
f"507-movements/227-chain-pully/sprocket-{num_links}teeth.obj"),
|
|
sprocket)
|
|
|
|
|
|
def main():
|
|
scene = {
|
|
"scene_type": "distance_barrier_rb_problem",
|
|
"solver": "ipc_solver",
|
|
"timestep": 0.01,
|
|
"max_time": 5.0,
|
|
"distance_barrier_constraint": {
|
|
"initial_barrier_activation_distance": 1e-3 * scale
|
|
},
|
|
"rigid_body_problem": {
|
|
"gravity": [0, -9.81, 0],
|
|
"rigid_bodies": []
|
|
}
|
|
}
|
|
|
|
num_links_1 = 20
|
|
num_links_2 = 8
|
|
|
|
cpoints1 = circle_points(num_links_1)
|
|
cpoints1 = cpoints1[:cpoints1.shape[0] // 2 + 1]
|
|
cpoints2 = circle_points(num_links_2)
|
|
cpoints2 = cpoints2[:cpoints2.shape[0] // 2 + 1]
|
|
cpoints2[:, 1] *= -1
|
|
|
|
dx = cpoints2[-1, 0] - cpoints1[-1, 0]
|
|
dlen = 10 * link_width
|
|
dy = numpy.sqrt(dlen**2 - dx**2)
|
|
cpoints2[:, 1] -= dy
|
|
|
|
dir1 = numpy.array([dx, -dy, 0])
|
|
lpoints1 = line_points(cpoints1[-1], dir1, 10)
|
|
dir2 = dir1.copy()
|
|
dir2[1] *= -1
|
|
lpoints2 = line_points(cpoints2[0], dir2, 10)
|
|
|
|
points = numpy.vstack(
|
|
[cpoints1[:], # circle
|
|
lpoints1[1:-1], # line down
|
|
cpoints2[::-1],
|
|
lpoints2[1:-1] # line up
|
|
])
|
|
|
|
R = numpy.array([[0, 1, 0],
|
|
[-1, 0, 0],
|
|
[0, 0, 1]])
|
|
points = points @ R.T
|
|
|
|
# export_polyline(points, offset=0, loops=True)
|
|
|
|
scene["rigid_body_problem"]["rigid_bodies"] = polyline_to_chain(points)
|
|
bodies = scene["rigid_body_problem"]["rigid_bodies"]
|
|
|
|
# generate_sprocket(num_links_1)
|
|
bodies.append({
|
|
"mesh": "507-movements/227-chain-pully/sprocket-20teeth.obj",
|
|
"angular_velocity": [0, 0, 100],
|
|
"scale": scale,
|
|
"type": "kinematic",
|
|
"is_dof_fixed": ([True] * 5 + [False])
|
|
})
|
|
|
|
# generate_sprocket(num_links_2)
|
|
bodies.append({
|
|
"mesh": "507-movements/227-chain-pully/sprocket-8teeth.obj",
|
|
"scale": scale,
|
|
"type": "dynamic",
|
|
"is_dof_fixed": ([True] * 5 + [False])
|
|
})
|
|
|
|
save_fixture(scene, get_fixture_dir_path() / "3D" /
|
|
"mechanisms/507-movements" / "227-chain-pully-scaled-up.json")
|
|
|
|
|
|
if __name__ == "__main__":
|
|
main()
|
|
|