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.
136 lines
4.3 KiB
136 lines
4.3 KiB
#!/usr/local/bin/python3
|
|
"""
|
|
Script to generate a fixture of a chain with N complex links.
|
|
|
|
Usage: python generate_complex_chainmail_fixture.py N
|
|
"""
|
|
|
|
import json
|
|
import pathlib
|
|
|
|
import numpy
|
|
import shapely.geometry
|
|
import shapely.ops
|
|
|
|
from fixture_utils import *
|
|
|
|
|
|
def generate_link_polygons() -> list:
|
|
"""Generate a list of Polygons for the chain link."""
|
|
half_thickness = 1e-2
|
|
width = 6
|
|
height = 5
|
|
|
|
head_hx = width / 2 - 4 * half_thickness - 0.5
|
|
foot_hx = width / 4 - 4 * half_thickness
|
|
leg_hy = 2 * height / 7 - half_thickness
|
|
leg_cy = leg_hy + half_thickness
|
|
torso_hx = width / 2
|
|
torso_cy = 2 * leg_cy - half_thickness
|
|
neck_hy = (height - torso_cy - half_thickness) / 2
|
|
neck_cy = height - neck_hy - half_thickness
|
|
|
|
area = half_thickness * (head_hx + neck_hy + torso_hx + leg_hy + foot_hx +
|
|
leg_hy + foot_hx) - 6 * half_thickness
|
|
|
|
return [
|
|
# Head
|
|
generate_rectangle(head_hx, half_thickness,
|
|
numpy.array([width / 2, height - half_thickness]),
|
|
0),
|
|
# Neck
|
|
generate_rectangle(half_thickness, neck_hy,
|
|
numpy.array([width / 2, neck_cy]), 0),
|
|
# Torso
|
|
generate_rectangle(torso_hx, half_thickness,
|
|
numpy.array([torso_hx, torso_cy]), 0),
|
|
# Left leg
|
|
generate_rectangle(half_thickness, leg_hy,
|
|
numpy.array([half_thickness, leg_cy]), 0),
|
|
# Left foot
|
|
generate_rectangle(foot_hx, half_thickness,
|
|
numpy.array([foot_hx, half_thickness]), 0),
|
|
# Right leg
|
|
generate_rectangle(half_thickness, leg_hy,
|
|
numpy.array([width - half_thickness, leg_cy]), 0),
|
|
# Right foot
|
|
generate_rectangle(foot_hx, half_thickness,
|
|
numpy.array([width - foot_hx, half_thickness]), 0),
|
|
], area
|
|
|
|
|
|
def generate_fixture(args: argparse.Namespace) -> dict:
|
|
"""Generate a fixture of a chain with N complex links."""
|
|
fixture = generate_custom_fixture(args)
|
|
rigid_bodies = fixture["rigid_body_problem"]["rigid_bodies"]
|
|
|
|
link_polygons, link_area = generate_link_polygons()
|
|
link = shapely.ops.cascaded_union(link_polygons)
|
|
link = shapely.geometry.polygon.orient(link, 1)
|
|
link_polygons = [
|
|
list(polygon.exterior.coords) for polygon in link_polygons
|
|
]
|
|
vertices = numpy.array(list(link.exterior.coords)[:-1])
|
|
|
|
angle = 90 + 45
|
|
theta = numpy.radians(angle)
|
|
R = create_2D_rotation_matrix(theta)
|
|
|
|
edges = generate_ngon_edges(vertices.shape[0])
|
|
|
|
link_mass = 0.1 # Kg
|
|
link_density = link_mass / link_area
|
|
|
|
for i in range(args.num_links):
|
|
rigid_bodies.append({
|
|
"vertices":
|
|
vertices.tolist(),
|
|
"polygons":
|
|
link_polygons,
|
|
"edges":
|
|
edges.tolist(),
|
|
"position": (R @ numpy.array([0, -3.5 * i])).tolist(),
|
|
"theta":
|
|
angle,
|
|
"velocity": [0.0, 0.0, 0.0],
|
|
"is_dof_fixed":
|
|
numpy.full(3, i == 0, dtype=bool).tolist(),
|
|
"oriented":
|
|
True,
|
|
"masses":
|
|
numpy.full(vertices.shape[0],
|
|
link_mass / vertices.shape[0]).tolist(),
|
|
"density":
|
|
link_density
|
|
})
|
|
|
|
return fixture
|
|
|
|
|
|
def main() -> None:
|
|
"""Parse command - line arguments to generate the desired fixture."""
|
|
parser = create_argument_parser("generate a chain fixture",
|
|
default_initial_epsilon=1e-3,
|
|
default_gravity=[0, -9.81, 0],
|
|
default_num_steps=5000)
|
|
parser.add_argument("--num-links",
|
|
type=int,
|
|
default=10,
|
|
help="number of links in the chain")
|
|
args = parser.parse_args()
|
|
|
|
if args.out_path is None:
|
|
directory = (pathlib.Path(__file__).resolve().parents[1] / "fixtures" /
|
|
"chain")
|
|
args.out_path = directory / f"{args.num_links:d}_link_chain.json"
|
|
args.out_path.parent.mkdir(parents=True, exist_ok=True)
|
|
|
|
print_args(args)
|
|
|
|
fixture = generate_fixture(args)
|
|
|
|
save_fixture(fixture, args.out_path)
|
|
|
|
|
|
if __name__ == "__main__":
|
|
main()
|
|
|