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.
387 lines
12 KiB
387 lines
12 KiB
# Script to test Rigid IPC examples in Project Chrono
|
|
# Adapted from a pychrono example originally from Alessandro Tasora
|
|
|
|
import json
|
|
import pathlib
|
|
import argparse
|
|
import subprocess
|
|
from datetime import datetime
|
|
|
|
import numpy
|
|
from scipy.spatial.transform import Rotation
|
|
|
|
import tqdm
|
|
|
|
import pychrono.core as chrono
|
|
import igl
|
|
|
|
|
|
timestepper_names = {
|
|
0: "EULER_IMPLICIT_LINEARIZED",
|
|
1: "EULER_IMPLICIT_PROJECTED",
|
|
2: "EULER_IMPLICIT",
|
|
3: "TRAPEZOIDAL",
|
|
4: "TRAPEZOIDAL_LINEARIZED",
|
|
5: "HHT",
|
|
6: "HEUN",
|
|
7: "RUNGEKUTTA45",
|
|
8: "EULER_EXPLICIT",
|
|
9: "LEAPFROG",
|
|
10: "NEWMARK",
|
|
20: "CUSTOM",
|
|
}
|
|
|
|
solver_names = {
|
|
0: "PSOR",
|
|
1: "PSSOR",
|
|
2: "PJACOBI",
|
|
3: "PMINRES",
|
|
4: "BARZILAIBORWEIN",
|
|
5: "APGD",
|
|
6: "ADDM",
|
|
7: "SPARSE_LU",
|
|
8: "SPARSE_QR",
|
|
9: "PARDISO_MKL",
|
|
10: "PARDISO_PROJECT",
|
|
11: "MUMPS",
|
|
12: "GMRES",
|
|
13: "MINRES",
|
|
14: "BICGSTAB",
|
|
15: "CUSTOM"
|
|
}
|
|
|
|
box_vertices = numpy.array([
|
|
[-0.5, -0.5, -0.5],
|
|
[0.5, 0.5, -0.5],
|
|
[0.5, -0.5, -0.5],
|
|
[-0.5, 0.5, -0.5],
|
|
[-0.5, 0.5, 0.5],
|
|
[-0.5, -0.5, 0.5],
|
|
[0.5, 0.5, 0.5],
|
|
[0.5, -0.5, 0.5]])
|
|
box_faces = numpy.array([
|
|
[0, 1, 2],
|
|
[0, 3, 1],
|
|
[0, 4, 3],
|
|
[0, 5, 4],
|
|
[3, 6, 1],
|
|
[3, 4, 6],
|
|
[2, 1, 6],
|
|
[2, 6, 7],
|
|
[0, 2, 7],
|
|
[0, 7, 5],
|
|
[5, 7, 6],
|
|
[5, 6, 4]], dtype=int)
|
|
|
|
|
|
def print_simulation_parameters(system):
|
|
print(f"""SolverType: {solver_names[system.GetSolverType()]},
|
|
TimestepperType: {timestepper_names[system.GetTimestepperType()]},
|
|
SolverMaxIterations: {system.GetSolverMaxIterations():d},
|
|
SolverTolerance: {system.GetSolverTolerance():g},
|
|
Maxiter: {system.GetMaxiter():d},
|
|
MaxPenetrationRecoverySpeed: {system.GetMaxPenetrationRecoverySpeed():g},
|
|
DefaultSuggestedEnvelope: {chrono.ChCollisionModel.GetDefaultSuggestedEnvelope():g},
|
|
DefaultSuggestedMargin: {chrono.ChCollisionModel.GetDefaultSuggestedMargin():g},
|
|
""")
|
|
|
|
|
|
def set_simulation_parameters(system, collision_envelope=None, collision_margin=None):
|
|
print("Default parameters:")
|
|
print_simulation_parameters(system)
|
|
|
|
# system.SetSolverType(chrono.ChSolver.Type_APGD)
|
|
system.SetSolverType(chrono.ChSolver.Type_BARZILAIBORWEIN)
|
|
system.SetTimestepperType(
|
|
chrono.ChTimestepper.Type_EULER_IMPLICIT_PROJECTED)
|
|
system.SetSolverMaxIterations(15000)
|
|
system.SetSolverTolerance(1e-12)
|
|
system.SetMaxiter(1000)
|
|
# system.SetMaxPenetrationRecoverySpeed(0.1)
|
|
|
|
# Set the global collision margins. This is expecially important for very large or
|
|
# very small objects. Set this before creating shapes. Not before creating system.
|
|
if collision_envelope is not None:
|
|
chrono.ChCollisionModel.SetDefaultSuggestedEnvelope(collision_envelope)
|
|
if collision_margin is not None:
|
|
chrono.ChCollisionModel.SetDefaultSuggestedMargin(collision_margin)
|
|
|
|
print("Using parameters:")
|
|
print_simulation_parameters(system)
|
|
|
|
|
|
def get_time_stamp():
|
|
return datetime.now().strftime("%Y-%b-%d-%H-%M-%S")
|
|
|
|
|
|
def get_trafos(body):
|
|
t = body.GetPos()
|
|
t = numpy.array([t.x, t.y, t.z])
|
|
R = numpy.zeros((3, 3))
|
|
for i in range(3):
|
|
for j in range(3):
|
|
R[i, j] = body.GetA()[i, j]
|
|
return t, R
|
|
|
|
|
|
def save_mesh(system, meshes, out_path, index):
|
|
bds = system.Get_bodylist()
|
|
|
|
Vs = []
|
|
Fs = []
|
|
offset = 0
|
|
|
|
for bi, body in enumerate(bds):
|
|
t, R = get_trafos(body)
|
|
V = meshes[bi][0].copy()
|
|
F = meshes[bi][1].copy()
|
|
|
|
V = V @ R.T
|
|
V += t
|
|
|
|
F += offset
|
|
offset += V.shape[0]
|
|
|
|
Vs.append(V)
|
|
Fs.append(F)
|
|
|
|
V = numpy.concatenate(Vs)
|
|
F = numpy.concatenate(Fs)
|
|
|
|
igl.write_triangle_mesh(str(out_path / f"m_{index:04d}.obj"), V, F)
|
|
|
|
|
|
def run_simulation(fixture, mesh_path, out_path, timestep=None,
|
|
collision_envelope=None, collision_margin=None, mu=None):
|
|
rb_problem = fixture["rigid_body_problem"]
|
|
|
|
system = chrono.ChSystemNSC()
|
|
system.Set_G_acc(chrono.ChVectorD(*rb_problem.get("gravity", [0, 0, 0])))
|
|
dhat = fixture.get(
|
|
"distance_barrier_constraint",
|
|
{"initial_barrier_activation_distance": 1e-3}).get(
|
|
"initial_barrier_activation_distance", 1e-3)
|
|
if collision_envelope is not None and collision_envelope < 0:
|
|
collision_envelope = dhat
|
|
if collision_margin is not None and collision_margin < 0:
|
|
collision_margin = dhat
|
|
set_simulation_parameters(system, collision_envelope, collision_margin)
|
|
|
|
# ---------------------------------------------------------------------
|
|
#
|
|
# Create the simulation system and add items
|
|
#
|
|
|
|
# Create a contact material (with default properties, shared by all collision shapes)
|
|
contact_material = chrono.ChMaterialSurfaceNSC()
|
|
|
|
if mu is None:
|
|
mu = rb_problem.get("coefficient_friction", 0)
|
|
contact_material.SetFriction(max(mu, 0))
|
|
# comp = system.GetMaterialCompositionStrategy()
|
|
|
|
meshes = []
|
|
|
|
for body in rb_problem["rigid_bodies"]:
|
|
mpath = str(mesh_path / body["mesh"])
|
|
|
|
if body["mesh"] == "plane.obj":
|
|
pos = body["position"]
|
|
dim = body.get("dimensions", [10, 0.005, 10])
|
|
rot = body.get("rotation", [0, 0, 0])
|
|
rot = Rotation.from_euler('xyz', rot, degrees=True)
|
|
rot = rot.as_quat()
|
|
|
|
floor = chrono.ChBodyEasyBox(*dim, 1, True, True, contact_material)
|
|
floor.SetPos(chrono.ChVectorD(*pos))
|
|
floor.SetRot(chrono.ChQuaternionD(rot[3], rot[0], rot[1], rot[2]))
|
|
if body.get("is_dof_fixed", False) or body.get("type", None) == "static":
|
|
floor.SetBodyFixed(True)
|
|
system.Add(floor)
|
|
meshes.append((dim * box_vertices, box_faces))
|
|
continue
|
|
|
|
if body.get("scale", 1) != 1:
|
|
v, f = igl.read_triangle_mesh(mpath)
|
|
v *= body["scale"]
|
|
igl.write_triangle_mesh("tmp.obj", v, f)
|
|
mpath = "tmp.obj"
|
|
|
|
density = body.get("density", 1000)
|
|
|
|
cbody = chrono.ChBodyEasyMesh(
|
|
mpath, # mesh filename
|
|
density, # density kg/m^3
|
|
True, # automatically compute mass and inertia?
|
|
True, # visualize?
|
|
True, # collide?
|
|
contact_material) # contact material
|
|
|
|
cbody.Update()
|
|
V, F = igl.read_triangle_mesh(mpath)
|
|
t, R = get_trafos(cbody)
|
|
# print(t)
|
|
# print(R)
|
|
V = V - t
|
|
V = V @ R
|
|
meshes.append((V, F))
|
|
|
|
# rot = Rotation.from_euler('zyx', body["rotation"], degrees=True)
|
|
rot2 = Rotation.from_matrix(R)
|
|
rotTmp = body.get("rotation", [0.0, 0.0, 0.0])
|
|
rot = Rotation.from_euler('xyz', rotTmp, degrees=True) * rot2
|
|
rot = rot.as_quat()
|
|
cbody.SetRot(chrono.ChQuaternionD(rot[3], rot[0], rot[1], rot[2]))
|
|
|
|
if "position" in body:
|
|
pos = body["position"]
|
|
pos = [x + y for (x, y) in zip(pos, t)]
|
|
cbody.SetPos(chrono.ChVectorD(pos[0], pos[1], pos[2]))
|
|
|
|
# Need to fix coordinate systems
|
|
if body.get("linear_velocity", [0, 0, 0]) != [0, 0, 0]:
|
|
raise NotImplementedError(
|
|
"Initial linear velocity not implemented!")
|
|
# vel = body["linear_velocity"]
|
|
# cbody.SetPos_dt(chrono.ChVectorD(vel[0], vel[1], vel[2]))
|
|
if body.get("angular_velocity", [0, 0, 0]) != [0, 0, 0]:
|
|
raise NotImplementedError(
|
|
"Initial angular velocity not implemented!")
|
|
# vel = body["angular_velocity"]
|
|
# cbody.SetWVel_loc(chrono.ChVectorD(vel[0], vel[1], vel[2]))
|
|
|
|
is_fixed = body.get("type", "dynamic") == "static"
|
|
if not is_fixed:
|
|
is_dof_fixed = body.get("is_dof_fixed", False)
|
|
if isinstance(is_dof_fixed, list):
|
|
is_fixed = all(is_dof_fixed)
|
|
if not is_fixed and any(is_dof_fixed):
|
|
raise NotImplementedError("Only all dofs fixed supported!")
|
|
else:
|
|
is_fixed = is_dof_fixed
|
|
cbody.SetBodyFixed(is_fixed)
|
|
|
|
cbody.Update()
|
|
system.Add(cbody)
|
|
|
|
if timestep is None:
|
|
timestep = fixture.get("timestep", 1e-2)
|
|
max_time = fixture.get("max_time", 10)
|
|
|
|
index = 0
|
|
prev_save = 0
|
|
save_mesh(system, meshes, out_path, 0)
|
|
|
|
for i in tqdm.tqdm(range(int(numpy.ceil(max_time / timestep)))):
|
|
system.DoStepDynamics(timestep)
|
|
if timestep >= 1e-2 or system.GetChTime() - prev_save > 1e-2:
|
|
index += 1
|
|
save_mesh(system, meshes, out_path, index)
|
|
prev_save = system.GetChTime()
|
|
|
|
|
|
def parse_args():
|
|
parser = argparse.ArgumentParser(
|
|
description="Test Rigid IPC examples in Project Chrono")
|
|
parser.add_argument(
|
|
"-i", "--input", metavar="path/to/input", type=pathlib.Path,
|
|
dest="input", help="path to input json(s)", nargs="+")
|
|
parser.add_argument(
|
|
"--chrono-data", metavar="path/to/chrono/data", type=str,
|
|
default="/usr/local/share/chrono/data/", dest="chrono_data_path",
|
|
help="path to Chrono data")
|
|
parser.add_argument(
|
|
"--dt", "--timestep", type=float, default=None, dest="timestep",
|
|
help="timestep")
|
|
parser.add_argument(
|
|
"--mu", type=float, default=None, dest="mu", help="coeff. friction")
|
|
|
|
def env_mar_to_float(x):
|
|
if x == "dhat":
|
|
return -1
|
|
return float(x)
|
|
|
|
parser.add_argument("--envelope", type=env_mar_to_float, default=None,
|
|
help="collision envelope value")
|
|
parser.add_argument("--margin", type=env_mar_to_float, default=None,
|
|
help="collision margin value")
|
|
parser.add_argument("--no-video", action="store_true", default=False,
|
|
help="skip rendering")
|
|
args = parser.parse_args()
|
|
|
|
if not pathlib.Path(args.chrono_data_path).exists():
|
|
parser.error(f"Invalid Chrono data path: {args.chrono_data_path}")
|
|
|
|
inputs = []
|
|
for input in args.input:
|
|
if input.is_file() and input.suffix == ".json":
|
|
inputs.append(input.resolve())
|
|
elif input.is_dir():
|
|
for glob_input in input.glob('**/*.json'):
|
|
inputs.append(glob_input.resolve())
|
|
args.input = inputs
|
|
|
|
return args
|
|
|
|
|
|
def main():
|
|
args = parse_args()
|
|
|
|
# The path to the Chrono data directory containing various assets (meshes,
|
|
# textures, data files) is automatically set, relative to the default
|
|
# location of this demo. If running from a different directory, you must
|
|
# change the path to the data directory with:
|
|
chrono.SetChronoDataPath(str(args.chrono_data_path))
|
|
|
|
root_path = pathlib.Path(__file__).resolve().parents[2]
|
|
mesh_path = root_path / "meshes"
|
|
|
|
renderer = root_path / "build" / "release" / "tools" / "render_simulation"
|
|
if not renderer.exists():
|
|
renderer = None
|
|
|
|
cwd_output = pathlib.Path("output").resolve()
|
|
|
|
for input in args.input:
|
|
with open(input) as f:
|
|
fixture = json.load(f)
|
|
|
|
if args.timestep is None:
|
|
args.timestep = fixture.get("timestep", 1e-2)
|
|
|
|
try:
|
|
out_path = input.resolve().relative_to(
|
|
root_path / "fixtures" / "3D").with_suffix("")
|
|
except:
|
|
out_path = input.stem
|
|
out_path = cwd_output / out_path
|
|
folder_name = "_".join(
|
|
([] if args.timestep is None else [f"timestep={args.timestep:g}"])
|
|
+ ([] if args.mu is None else [f"mu={args.mu:g}"]))
|
|
out_path /= folder_name
|
|
print("out_path:", out_path)
|
|
out_path.mkdir(exist_ok=True, parents=True)
|
|
|
|
try:
|
|
run_simulation(fixture, mesh_path, out_path,
|
|
timestep=args.timestep,
|
|
collision_envelope=args.envelope,
|
|
collision_margin=args.margin,
|
|
mu=args.mu)
|
|
|
|
# Render simulation
|
|
if renderer is not None and not args.no_video:
|
|
print("Rendering simulation")
|
|
video_name = f"{input.stem}-{get_time_stamp()}-chrono.mp4"
|
|
subprocess.run([str(renderer),
|
|
"-i", out_path,
|
|
"-o", out_path / video_name,
|
|
"--fps", "100"])
|
|
except NotImplementedError as err:
|
|
print(f"{input}: {err}")
|
|
out_path.rmdir()
|
|
print()
|
|
|
|
|
|
if __name__ == "__main__":
|
|
main()
|
|
|