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.
393 lines
13 KiB
393 lines
13 KiB
import json
|
|
import argparse
|
|
import pathlib
|
|
import time
|
|
import datetime
|
|
import subprocess
|
|
|
|
import numpy
|
|
import tqdm
|
|
import pybullet as bullet
|
|
import igl
|
|
|
|
|
|
def get_time_stamp():
|
|
return datetime.datetime.now().strftime("%Y-%b-%d-%H-%M-%S")
|
|
|
|
|
|
def save_mesh(meshes, out_path, index):
|
|
Vs = []
|
|
Fs = []
|
|
offset = 0
|
|
|
|
for i in range(bullet.getNumBodies()):
|
|
try:
|
|
V = meshes[i][0].copy()
|
|
except:
|
|
breakpoint()
|
|
F = meshes[i][1].copy()
|
|
|
|
t, q = bullet.getBasePositionAndOrientation(i)
|
|
R = numpy.array(bullet.getMatrixFromQuaternion(q)).reshape(3, 3)
|
|
|
|
V = V @ R.T + 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 object_from_obj(filename, mass=1, mesh_scale=[1, 1, 1]):
|
|
col_flags = bullet.URDF_INITIALIZE_SAT_FEATURES
|
|
if mass == 0: # static objects
|
|
col_flags = bullet.GEOM_FORCE_CONCAVE_TRIMESH
|
|
|
|
collision_shape_id = bullet.createCollisionShape(
|
|
shapeType=bullet.GEOM_MESH,
|
|
fileName=str(filename),
|
|
collisionFramePosition=[0, 0, 0],
|
|
meshScale=mesh_scale,
|
|
flags=col_flags)
|
|
|
|
visual_shape_id = bullet.createVisualShape(
|
|
shapeType=bullet.GEOM_MESH,
|
|
fileName=str(filename),
|
|
visualFramePosition=[0, 0, 0],
|
|
meshScale=mesh_scale)
|
|
|
|
body = bullet.createMultiBody(
|
|
baseMass=mass,
|
|
baseInertialFramePosition=[0, 0, 0],
|
|
baseCollisionShapeIndex=collision_shape_id,
|
|
baseVisualShapeIndex=visual_shape_id,
|
|
basePosition=[0, 0, 0])
|
|
|
|
average_vertex = numpy.zeros(3)
|
|
if mass > 0:
|
|
# compute inertial frame, assuming mass at vertices
|
|
average_vertex = numpy.average(
|
|
numpy.array(bullet.getMeshData(body)[1]), axis=0)
|
|
|
|
# bullet.resetBasePositionAndOrientation(body, [100000,0,0],[0,0,0,1])
|
|
bullet.removeBody(body)
|
|
shift = -average_vertex
|
|
|
|
collision_shape_id2 = bullet.createCollisionShape(
|
|
shapeType=bullet.GEOM_MESH,
|
|
fileName=str(filename),
|
|
collisionFramePosition=shift,
|
|
meshScale=mesh_scale,
|
|
flags=col_flags)
|
|
visual_shape_id2 = bullet.createVisualShape(
|
|
shapeType=bullet.GEOM_MESH,
|
|
fileName=str(filename),
|
|
visualFramePosition=shift,
|
|
meshScale=mesh_scale)
|
|
|
|
body = bullet.createMultiBody(
|
|
baseMass=mass,
|
|
baseInertialFramePosition=[0, 0, 0],
|
|
baseCollisionShapeIndex=collision_shape_id2,
|
|
baseVisualShapeIndex=visual_shape_id2,
|
|
basePosition=[0, 0, 0])
|
|
|
|
if mass > 0:
|
|
# make dynamics objects random color
|
|
color = (numpy.random.random(3)).tolist() + [1]
|
|
bullet.changeVisualShape(body, -1, rgbaColor=color)
|
|
|
|
bullet.changeVisualShape(
|
|
body, -1, flags=bullet.VISUAL_SHAPE_DOUBLE_SIDED)
|
|
|
|
return body, average_vertex
|
|
|
|
|
|
def convert_to_convex_mesh(in_path, out_path):
|
|
bullet.vhacd(
|
|
str(in_path), str(out_path), "vhacd_log.txt", concavity=0,
|
|
maxNumVerticesPerCH=1024, depth=32, resolution=1000000,
|
|
convexhullApproximation=0)
|
|
|
|
|
|
def print_simulation_parameters():
|
|
for param, val in bullet.getPhysicsEngineParameters().items():
|
|
print(f"{param}: {val}")
|
|
print()
|
|
|
|
|
|
def run_simulation(fixture, meshes_path, out_path, args):
|
|
rigid_body_problem = fixture["rigid_body_problem"]
|
|
|
|
gravity = rigid_body_problem.get("gravity", [0, 0, 0])
|
|
|
|
timestep = args.timestep
|
|
if timestep is None:
|
|
timestep = fixture.get("timestep", 1e-2)
|
|
|
|
if args.use_gui:
|
|
bullet.connect(bullet.GUI)
|
|
bullet.configureDebugVisualizer(flag=bullet.COV_ENABLE_Y_AXIS_UP)
|
|
bullet.configureDebugVisualizer(bullet.COV_ENABLE_RENDERING, 0)
|
|
else:
|
|
bullet.connect(bullet.DIRECT)
|
|
|
|
print("Default parameters:")
|
|
print_simulation_parameters()
|
|
|
|
bullet.setPhysicsEngineParameter(
|
|
numSolverIterations=2500,
|
|
solverResidualThreshold=1e-12,
|
|
enableSAT=args.enable_sat,
|
|
enableConeFriction=args.enable_cone_friction)
|
|
if args.use_ccd:
|
|
bullet.setPhysicsEngineParameter(allowedCcdPenetration=0.0)
|
|
bullet.setGravity(*gravity)
|
|
bullet.setTimeStep(timestep)
|
|
|
|
# plane = bullet.loadURDF("plane_implicit.urdf")#, pos = object_from_obj("meshes/plane.obj", mass=0)
|
|
# bullet.changeDynamics(plane,-1,lateralFriction=1, frictionAnchor = 1, contactStiffness=30000, contactDamping=10000)
|
|
# orn = bullet.getQuaternionFromEuler([ -numpy.pi/2,0, 0])
|
|
# bullet.resetBasePositionAndOrientation(plane, [0,0,0], orn)
|
|
|
|
meshes = []
|
|
|
|
convex_meshes_path = pathlib.Path(__file__).resolve().parent / "meshes"
|
|
|
|
# combined_friction = friction_a * friction_b so take the sqrt
|
|
mu = args.mu
|
|
if mu is None:
|
|
mu = rigid_body_problem.get("coefficient_friction", 0.0)
|
|
mu = numpy.sqrt(mu)
|
|
|
|
for body in rigid_body_problem["rigid_bodies"]:
|
|
if not body.get("enabled", True):
|
|
continue
|
|
|
|
mesh_path = meshes_path / body["mesh"]
|
|
|
|
mass = body.get("density", 1000) # Assumes the volume is 1 m³
|
|
|
|
is_dof_fixed = body.get("is_dof_fixed", False)
|
|
if (body.get("type", "dynamic") == "static"
|
|
or (isinstance(is_dof_fixed, list) and all(is_dof_fixed))
|
|
or is_dof_fixed):
|
|
mass = 0 # static object
|
|
|
|
if args.make_convex and mass > 0:
|
|
try:
|
|
convex_mesh_path = (
|
|
convex_meshes_path / mesh_path.relative_to(meshes_path))
|
|
convex_mesh_path.parent.mkdir(parents=True, exist_ok=True)
|
|
except:
|
|
convex_mesh_path = convex_meshes_path / mesh_path.name
|
|
if not convex_mesh_path.exists():
|
|
convert_to_convex_mesh(mesh_path, convex_mesh_path)
|
|
mesh_path = convex_mesh_path
|
|
|
|
V, F = igl.read_triangle_mesh(str(mesh_path))
|
|
mesh_scale = body.get("scale", [1, 1, 1])
|
|
if isinstance(mesh_scale, float):
|
|
mesh_scale = [mesh_scale] * 3
|
|
if "dimensions" in body:
|
|
org_dim = V.max(axis=0) - V.min(axis=0)
|
|
org_dim[org_dim <= 0] = 1
|
|
mesh_scale = body["dimensions"] / org_dim
|
|
|
|
body_id, com_shift = object_from_obj(
|
|
mesh_path, mass=mass, mesh_scale=mesh_scale)
|
|
|
|
bullet.changeDynamics(
|
|
body_id, -1, lateralFriction=mu, frictionAnchor=False)
|
|
if args.use_ccd:
|
|
bullet.changeDynamics(
|
|
body_id, -1, ccdSweptSphereRadius=args.ccd_radius)
|
|
|
|
pos = body.get("position", [0, 0, 0])
|
|
eul = numpy.deg2rad(body.get("rotation", [0, 0, 0]))
|
|
orn = bullet.getQuaternionFromEuler(eul)
|
|
|
|
lin_vel = body.get("linear_velocity", [0, 0, 0])
|
|
ang_vel = numpy.deg2rad(body.get("angular_velocity", [0, 0, 0]))
|
|
|
|
com_pos = pos + args.shift_mag * com_shift
|
|
meshes.append((V * mesh_scale - args.shift_mag * com_shift, F))
|
|
|
|
bullet.resetBasePositionAndOrientation(body_id, com_pos, orn)
|
|
bullet.resetBaseVelocity(body_id, lin_vel, ang_vel)
|
|
|
|
max_steps = int(numpy.ceil(fixture.get("max_time", 5) / timestep))
|
|
|
|
if args.use_gui:
|
|
bullet.configureDebugVisualizer(bullet.COV_ENABLE_RENDERING, 1)
|
|
cameraPitch = 0.
|
|
cameraYaw = -180.
|
|
bullet.resetDebugVisualizerCamera(
|
|
cameraDistance=args.camera_distance,
|
|
cameraYaw=cameraYaw,
|
|
cameraPitch=cameraPitch,
|
|
cameraTargetPosition=args.camera_target)
|
|
|
|
index = 0
|
|
save_mesh(meshes, out_path, 0)
|
|
skip_frames = 0 if timestep >= 1e-2 else 1e-2 / timestep
|
|
prev_save = 0
|
|
if args.use_gui:
|
|
prev_step = 0
|
|
step_id = bullet.addUserDebugParameter("Step", 1, -1, prev_step)
|
|
prev_run = 0
|
|
run_id = bullet.addUserDebugParameter("Run", 1, -1, prev_run)
|
|
run_sim = False
|
|
|
|
print("Using parameters:")
|
|
print_simulation_parameters()
|
|
|
|
pbar = tqdm.tqdm(total=(max_steps + 1))
|
|
i = 0
|
|
while (args.use_gui and bullet.isConnected()) or i <= max_steps:
|
|
take_step = not args.use_gui or run_sim
|
|
if args.use_gui:
|
|
step = bullet.readUserDebugParameter(step_id)
|
|
if step != prev_step:
|
|
prev_step = step
|
|
take_step = True
|
|
|
|
run = bullet.readUserDebugParameter(run_id)
|
|
if run != prev_run:
|
|
prev_run = run
|
|
run_sim = not run_sim
|
|
take_step = run_sim
|
|
|
|
if take_step:
|
|
bullet.stepSimulation()
|
|
pbar.update(1)
|
|
if i - prev_save >= skip_frames:
|
|
index += 1
|
|
save_mesh(meshes, out_path, index)
|
|
prev_save = i
|
|
|
|
i += 1
|
|
# if args.use_gui:
|
|
# time.sleep(1e-3)
|
|
pbar.close()
|
|
|
|
|
|
def parse_args():
|
|
parser = argparse.ArgumentParser(
|
|
description="Test Rigid IPC examples in Bullet")
|
|
parser.add_argument(
|
|
"-i", "--input", "--json-file", metavar="path/to/input",
|
|
type=pathlib.Path, dest="input", help="path to input json(s)",
|
|
nargs="+")
|
|
parser.add_argument(
|
|
"--shift-mag", type=int, default=1,
|
|
help=("Shift the collision/visual (due to OBJ file not centered) "
|
|
"values -1=negative shift, 0=no shift, 1=positive shift"))
|
|
parser.add_argument(
|
|
"--camera-distance", type=float, default=7,
|
|
help="Camera Distance (to target)")
|
|
parser.add_argument(
|
|
"--camera-target", type=float, default=[-1, 1, 1], nargs=3,
|
|
help="Camera target (lookat) position")
|
|
parser.add_argument(
|
|
"--enable-sat", action="store_true", default=False,
|
|
dest="enable_sat", help="Enable Separating Axis Test (SAT) collision")
|
|
parser.add_argument(
|
|
"--disable-cone-friction", action="store_false", default=True,
|
|
dest="enable_cone_friction",
|
|
help="Disable Cone friction (instead use the pyramid friction model)")
|
|
parser.add_argument(
|
|
"--make-convex", action="store_true", default=False,
|
|
help="Convert dynamic bodies to convex meshes (using V-HACD)")
|
|
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")
|
|
parser.add_argument(
|
|
"--use-ccd", action="store_true", default=False, dest="use_ccd",
|
|
help="enable CCD using swept spheres")
|
|
parser.add_argument(
|
|
"--ccd-radius", type=float, default=0.002, dest="ccd_radius",
|
|
help="CCD swept sphere radius")
|
|
parser.add_argument(
|
|
"--erp", type=float, default=None,
|
|
help="error reduction parameter")
|
|
parser.add_argument("--no-video", action="store_true", default=False,
|
|
help="skip rendering")
|
|
parser.add_argument("--use-gui", action="store_true", default=False,
|
|
help="use Bullet GUI")
|
|
args = parser.parse_args()
|
|
|
|
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()
|
|
|
|
root_path = pathlib.Path(__file__).resolve().parents[2]
|
|
meshes_path = root_path / "meshes"
|
|
|
|
renderer = root_path / "build" / "release" / "tools" / "render_simulation"
|
|
if not renderer.exists():
|
|
renderer = None
|
|
|
|
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.relative_to(
|
|
root_path / "fixtures" / "3D").with_suffix("")
|
|
except:
|
|
out_path = input.stem
|
|
out_path = ("output" / out_path).resolve()
|
|
folder_name = "_".join(
|
|
([] if args.timestep is None else [f"timestep={args.timestep:g}"])
|
|
+ (["sat"] if args.enable_sat else [])
|
|
+ ([] 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, meshes_path, out_path, args)
|
|
except Exception as e:
|
|
print(e)
|
|
try:
|
|
bullet.resetSimulation()
|
|
except Exception as e:
|
|
print(e)
|
|
|
|
# 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"])
|
|
|
|
print()
|
|
|
|
|
|
if __name__ == "__main__":
|
|
main()
|
|
|