diff --git a/warp/examples/assets/square_cloth.usd b/warp/examples/assets/square_cloth.usd new file mode 100644 index 000000000..96acfde4b --- /dev/null +++ b/warp/examples/assets/square_cloth.usd @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:efe035f1cc801e4055fd4d9945c5aa46bd7777b58d8976563aef7a973ffdfbd7 +size 324952 diff --git a/warp/examples/sim/example_cloth_self_contact.py b/warp/examples/sim/example_cloth_self_contact.py new file mode 100644 index 000000000..614d07c01 --- /dev/null +++ b/warp/examples/sim/example_cloth_self_contact.py @@ -0,0 +1,260 @@ +# Copyright (c) 2024 NVIDIA CORPORATION. All rights reserved. +# NVIDIA CORPORATION and its licensors retain all intellectual property +# and proprietary rights in and to this software, related documentation +# and any modifications thereto. Any use, reproduction, disclosure or +# distribution of this software and related documentation without an express +# license agreement from NVIDIA CORPORATION is strictly prohibited. + +########################################################################### +# Example Sim Cloth Self Contact +# +# This simulation demonstrates twisting an FEM cloth model using the VBD +# integrator, showcasing its ability to handle complex self-contacts while +# ensuring it remains intersection-free. +# +########################################################################### + +import math +import os + +import numpy as np +from pxr import Usd, UsdGeom + +import warp as wp +import warp.examples +import warp.sim +import warp.sim.render +from warp.sim.model import PARTICLE_FLAG_ACTIVE + + +@wp.kernel +def initialize_rotation( + particle_indices_to_rot: wp.array(dtype=wp.int32), + pos: wp.array(dtype=wp.vec3), + rot_centers: wp.array(dtype=wp.vec3), + rot_axes: wp.array(dtype=wp.vec3), + roots: wp.array(dtype=wp.vec3), + roots_to_ps: wp.array(dtype=wp.vec3), +): + tid = wp.tid() + particle_index = particle_indices_to_rot[wp.tid()] + + p = pos[particle_index] + rot_center = rot_centers[tid] + rot_axis = rot_axes[tid] + op = p - rot_center + + root = wp.dot(op, rot_axis) * rot_axis + + root_to_p = p - root + + roots[tid] = root + roots_to_ps[tid] = root_to_p + + +@wp.kernel +def apply_rotation( + time: float, + angular_velocity: float, + particle_indices_to_rot: wp.array(dtype=wp.int32), + rot_centers: wp.array(dtype=wp.vec3), + rot_axes: wp.array(dtype=wp.vec3), + roots: wp.array(dtype=wp.vec3), + roots_to_ps: wp.array(dtype=wp.vec3), + pos_0: wp.array(dtype=wp.vec3), + pos_1: wp.array(dtype=wp.vec3), +): + tid = wp.tid() + particle_index = particle_indices_to_rot[wp.tid()] + + rot_axis = rot_axes[tid] + + ux = rot_axis[0] + uy = rot_axis[1] + uz = rot_axis[2] + + theta = time * angular_velocity + + R = wp.mat33( + wp.cos(theta) + ux * ux * (1.0 - wp.cos(theta)), + ux * uy * (1.0 - wp.cos(theta)) - uz * wp.sin(theta), + ux * uz * (1.0 - wp.cos(theta)) + uy * wp.sin(theta), + uy * ux * (1.0 - wp.cos(theta)) + uz * wp.sin(theta), + wp.cos(theta) + uy * uy * (1.0 - wp.cos(theta)), + uy * uz * (1.0 - wp.cos(theta)) - ux * wp.sin(theta), + uz * ux * (1.0 - wp.cos(theta)) - uy * wp.sin(theta), + uz * uy * (1.0 - wp.cos(theta)) + ux * wp.sin(theta), + wp.cos(theta) + uz * uz * (1.0 - wp.cos(theta)), + ) + + root = roots[tid] + root_to_p = roots_to_ps[tid] + root_to_p_rot = R * root_to_p + p_rot = root + root_to_p_rot + + pos_0[particle_index] = p_rot + pos_1[particle_index] = p_rot + + +class Example: + def __init__(self, stage_path="example_cloth_self_contact.usd", num_frames=1500): + fps = 60 + self.frame_dt = 1.0 / fps + self.num_substeps = 10 + self.iterations = 10 + self.dt = self.frame_dt / self.num_substeps + + self.num_frames = num_frames + self.sim_time = 0.0 + self.profiler = {} + + self.rot_angular_velocity = math.pi / 6 + self.rot_end_time = 21 + + usd_stage = Usd.Stage.Open(os.path.join(warp.examples.get_asset_directory(), "square_cloth.usd")) + usd_geom = UsdGeom.Mesh(usd_stage.GetPrimAtPath("/root/cloth/cloth")) + + mesh_points = np.array(usd_geom.GetPointsAttr().Get()) + mesh_indices = np.array(usd_geom.GetFaceVertexIndicesAttr().Get()) + + self.input_scale_factor = 1.0 + self.renderer_scale_factor = 0.01 + + vertices = [wp.vec3(v) * self.input_scale_factor for v in mesh_points] + self.faces = mesh_indices.reshape(-1, 3) + + builder = wp.sim.ModelBuilder() + builder.add_cloth_mesh( + pos=wp.vec3(0.0, 0.0, 0.0), + rot=wp.quat_identity(), + scale=1.0, + vertices=vertices, + indices=mesh_indices, + vel=wp.vec3(0.0, 0.0, 0.0), + density=0.02, + tri_ke=1.0e5, + tri_ka=1.0e5, + tri_kd=3.0e-5, + ) + builder.color() + self.model = builder.finalize() + self.model.ground = False + self.model.soft_contact_ke = 1.0e5 + self.model.soft_contact_kd = 1.0e-6 + self.model.soft_contact_mu = 0.2 + + # set up contact query and contact detection distances + self.model.soft_contact_radius = 0.2 + self.model.soft_contact_margin = 0.35 + + cloth_size = 50 + left_side = [cloth_size - 1 + i * cloth_size for i in range(cloth_size)] + right_side = [i * cloth_size for i in range(cloth_size)] + rot_point_indices = left_side + right_side + + if len(rot_point_indices): + flags = self.model.particle_flags.numpy() + for fixed_vertex_id in rot_point_indices: + flags[fixed_vertex_id] = wp.uint32(int(flags[fixed_vertex_id]) & ~int(PARTICLE_FLAG_ACTIVE)) + + self.model.particle_flags = wp.array(flags) + + self.integrator = wp.sim.VBDIntegrator( + self.model, + self.iterations, + handle_self_contact=True, + ) + self.state0 = self.model.state() + self.state1 = self.model.state() + + rot_axes = [[1, 0, 0]] * len(right_side) + [[-1, 0, 0]] * len(left_side) + + self.rot_point_indices = wp.array(rot_point_indices, dtype=int) + self.rot_centers = wp.zeros(len(rot_point_indices), dtype=wp.vec3) + self.rot_axes = wp.array(rot_axes, dtype=wp.vec3) + + self.roots = wp.zeros_like(self.rot_centers) + self.roots_to_ps = wp.zeros_like(self.rot_centers) + + wp.launch( + kernel=initialize_rotation, + dim=self.rot_point_indices.shape[0], + inputs=[ + self.rot_point_indices, + self.state0.particle_q, + self.rot_centers, + self.rot_axes, + self.roots, + self.roots_to_ps, + ], + ) + + if stage_path: + self.renderer = wp.sim.render.SimRenderer(self.model, stage_path, scaling=40.0) + else: + self.renderer = None + + def step(self): + with wp.ScopedTimer("step", print=False, dict=self.profiler): + for _ in range(self.num_substeps): + if self.sim_time < self.rot_end_time: + wp.launch( + kernel=apply_rotation, + dim=self.rot_point_indices.shape[0], + inputs=[ + self.sim_time, + self.rot_angular_velocity, + self.rot_point_indices, + self.rot_centers, + self.rot_axes, + self.roots, + self.roots_to_ps, + self.state0.particle_q, + self.state1.particle_q, + ], + ) + + self.integrator.simulate(self.model, self.state0, self.state1, self.dt) + + (self.state0, self.state1) = (self.state1, self.state0) + + self.sim_time += self.dt + + def render(self): + if self.renderer is None: + return + + with wp.ScopedTimer("render", print=False): + self.renderer.begin_frame(self.sim_time) + self.renderer.render(self.state0) + self.renderer.end_frame() + + +if __name__ == "__main__": + import argparse + + parser = argparse.ArgumentParser(formatter_class=argparse.ArgumentDefaultsHelpFormatter) + parser.add_argument("--device", type=str, default=None, help="Override the default Warp device.") + parser.add_argument( + "--stage_path", + type=lambda x: None if x == "None" else str(x), + default="example_cloth_self_contact.usd", + help="Path to the output USD file.", + ) + parser.add_argument("--num_frames", type=int, default=1500, help="Total number of frames.") + + args = parser.parse_known_args()[0] + + with wp.ScopedDevice(args.device): + example = Example(stage_path=args.stage_path, num_frames=args.num_frames) + + for i in range(example.num_frames): + example.step() + example.render() + print(f"[{i:4d}/{example.num_frames}]") + + frame_times = example.profiler["step"] + print("\nAverage frame sim time: {:.2f} ms".format(sum(frame_times) / len(frame_times))) + + if example.renderer: + example.renderer.save() diff --git a/warp/sim/collide.py b/warp/sim/collide.py index 8def4e86e..b35757444 100644 --- a/warp/sim/collide.py +++ b/warp/sim/collide.py @@ -16,6 +16,7 @@ from .model import PARTICLE_FLAG_ACTIVE, ModelShapeGeometry +# types of triangle's closest point to a point TRI_CONTACT_FEATURE_VERTEX_A = wp.constant(0) TRI_CONTACT_FEATURE_VERTEX_B = wp.constant(1) TRI_CONTACT_FEATURE_VERTEX_C = wp.constant(2) @@ -24,6 +25,45 @@ TRI_CONTACT_FEATURE_EDGE_BC = wp.constant(5) TRI_CONTACT_FEATURE_FACE_INTERIOR = wp.constant(6) +# constants used to access TriMeshCollisionDetector.resize_flags +VERTEX_COLLISION_BUFFER_OVERFLOW_INDEX = wp.constant(0) +TRI_COLLISION_BUFFER_OVERFLOW_INDEX = wp.constant(1) +EDGE_COLLISION_BUFFER_OVERFLOW_INDEX = wp.constant(2) +TRI_TRI_COLLISION_BUFFER_OVERFLOW_INDEX = wp.constant(3) + + +@wp.func +def build_orthonormal_basis(n: wp.vec3): + """ + Builds an orthonormal basis given a normal vector `n`. Return the two axes that are perpendicular to `n`. + + :param n: A 3D vector (list or array-like) representing the normal vector + """ + b1 = wp.vec3() + b2 = wp.vec3() + if n[2] < 0.0: + a = 1.0 / (1.0 - n[2]) + b = n[0] * n[1] * a + b1[0] = 1.0 - n[0] * n[0] * a + b1[1] = -b + b1[2] = n[0] + + b2[0] = b + b2[1] = n[1] * n[1] * a - 1.0 + b2[2] = -n[1] + else: + a = 1.0 / (1.0 + n[2]) + b = -n[0] * n[1] * a + b1[0] = 1.0 - n[0] * n[0] * a + b1[1] = b + b1[2] = -n[0] + + b2[0] = b + b2[1] = 1.0 - n[1] * n[1] * a + b2[2] = -n[1] + + return b1, b2 + @wp.func def triangle_closest_point_barycentric(a: wp.vec3, b: wp.vec3, c: wp.vec3, p: wp.vec3): @@ -1679,6 +1719,18 @@ def collide(model, state, edge_sdf_iter: int = 10, iterate_mesh_vertices: bool = ) +@wp.func +def compute_tri_aabb( + v1: wp.vec3, + v2: wp.vec3, + v3: wp.vec3, +): + lower = wp.min(wp.min(v1, v2), v3) + upper = wp.max(wp.max(v1, v2), v3) + + return lower, upper + + @wp.kernel def compute_tri_aabbs( pos: wp.array(dtype=wp.vec3), @@ -1692,8 +1744,10 @@ def compute_tri_aabbs( v2 = pos[tri_indices[t_id, 1]] v3 = pos[tri_indices[t_id, 2]] - lower_bounds[t_id] = wp.min(wp.min(v1, v2), v3) - upper_bounds[t_id] = wp.max(wp.max(v1, v2), v3) + upper, lower = compute_tri_aabb(v1, v2, v3) + + lower_bounds[t_id] = lower + upper_bounds[t_id] = upper @wp.kernel @@ -1759,7 +1813,7 @@ def vertex_triangle_collision_detection_kernel( pos: wp.array(dtype=wp.vec3), tri_indices: wp.array(dtype=wp.int32, ndim=2), vertex_colliding_triangles_offsets: wp.array(dtype=wp.int32), - vertex_colliding_triangles_buffer_size: wp.array(dtype=wp.int32), + vertex_colliding_triangles_buffer_sizes: wp.array(dtype=wp.int32), triangle_colliding_vertices_offsets: wp.array(dtype=wp.int32), triangle_colliding_vertices_buffer_sizes: wp.array(dtype=wp.int32), # outputs @@ -1792,7 +1846,7 @@ def vertex_triangle_collision_detection_kernel( vertex_colliding_triangles (array): flattened buffer of vertices' collision triangles vertex_colliding_triangles_count (array): number of triangles each vertex collides vertex_colliding_triangles_offsets (array): where each vertex' collision buffer starts - vertex_colliding_triangles_buffer_size (array): size of each vertex' collision buffer, will be modified if resizing is needed + vertex_colliding_triangles_buffer_sizes (array): size of each vertex' collision buffer, will be modified if resizing is needed vertex_colliding_triangles_min_dist (array): each vertex' min distance to all (non-neighbor) triangles triangle_colliding_vertices (array): positions of all the triangles' collision vertices triangle_colliding_vertices_count (array): number of triangles each vertex collides @@ -1837,7 +1891,7 @@ def vertex_triangle_collision_detection_kernel( if vertex_num_collisions < vertex_buffer_size: vertex_colliding_triangles[vertex_buffer_offset + vertex_num_collisions] = tri_index else: - resize_flags[0] = 1 + resize_flags[VERTEX_COLLISION_BUFFER_OVERFLOW_INDEX] = 1 vertex_num_collisions = vertex_num_collisions + 1 @@ -1850,14 +1904,14 @@ def vertex_triangle_collision_detection_kernel( # record v-f collision to triangle triangle_colliding_vertices[tri_buffer_offset + tri_num_collisions] = v_index else: - resize_flags[1] = 1 + resize_flags[TRI_COLLISION_BUFFER_OVERFLOW_INDEX] = 1 vertex_colliding_triangles_count[v_index] = vertex_num_collisions vertex_colliding_triangles_min_dist[v_index] = min_dis_to_tris @wp.kernel -def edge_edge_collision_detection_kernel( +def edge_colliding_edges_detection_kernel( query_radius: float, bvh_id: wp.uint64, pos: wp.array(dtype=wp.vec3), @@ -1876,7 +1930,7 @@ def edge_edge_collision_detection_kernel( query_radius (float): pos (array): positions of all the vertices that make up edges edge_colliding_triangles (array): flattened buffer of edges' collision edges - edge_colliding_edges_counts (array): number of edges each edge collides + edge_colliding_edges_count (array): number of edges each edge collides edge_colliding_triangles_offsets (array): where each edge's collision buffer starts edge_colliding_triangles_buffer_size (array): size of each edge's collision buffer, will be modified if resizing is needed edge_min_dis_to_triangles (array): each vertex' min distance to all (non-neighbor) triangles @@ -1927,7 +1981,7 @@ def edge_edge_collision_detection_kernel( if edge_num_collisions < edge_buffer_size: edge_colliding_edges[edge_buffer_offset + edge_num_collisions] = colliding_edge_index else: - resize_flags[1] = 1 + resize_flags[EDGE_COLLISION_BUFFER_OVERFLOW_INDEX] = 1 edge_num_collisions = edge_num_collisions + 1 @@ -1935,19 +1989,134 @@ def edge_edge_collision_detection_kernel( edge_colliding_edges_min_dist[e_index] = min_dis_to_edges +@wp.kernel +def triangle_triangle_collision_detection_kernel( + mesh_id: wp.uint64, + pos: wp.array(dtype=wp.vec3), + tri_indices: wp.array(dtype=wp.int32, ndim=2), + triangle_intersecting_triangles_offsets: wp.array(dtype=wp.int32), + # outputs + triangle_intersecting_triangles: wp.array(dtype=wp.int32), + triangle_intersecting_triangles_count: wp.array(dtype=wp.int32), + resize_flags: wp.array(dtype=wp.int32), +): + tri_index = wp.tid() + t1_v1 = tri_indices[tri_index, 0] + t1_v2 = tri_indices[tri_index, 1] + t1_v3 = tri_indices[tri_index, 2] + + v1 = pos[t1_v1] + v2 = pos[t1_v2] + v3 = pos[t1_v3] + + lower, upper = compute_tri_aabb(v1, v2, v3) + + buffer_offset = triangle_intersecting_triangles_offsets[tri_index] + buffer_size = triangle_intersecting_triangles_offsets[tri_index + 1] - buffer_offset + + query = wp.mesh_query_aabb(mesh_id, lower, upper) + tri_index_2 = wp.int32(0) + intersection_count = wp.int32(0) + while wp.mesh_query_aabb_next(query, tri_index_2): + t2_v1 = tri_indices[tri_index_2, 0] + t2_v2 = tri_indices[tri_index_2, 1] + t2_v3 = tri_indices[tri_index_2, 2] + + # filter out intersection test with neighbor triangles + if ( + vertex_adjacent_to_triangle(t1_v1, t2_v1, t2_v2, t2_v3) + or vertex_adjacent_to_triangle(t1_v2, t2_v1, t2_v2, t2_v3) + or vertex_adjacent_to_triangle(t1_v3, t2_v1, t2_v2, t2_v3) + ): + continue + + u1 = pos[t2_v1] + u2 = pos[t2_v2] + u3 = pos[t2_v3] + + if wp.intersect_tri_tri(v1, v2, v3, u1, u2, u3): + if intersection_count < buffer_size: + triangle_intersecting_triangles[buffer_offset + intersection_count] = tri_index_2 + else: + resize_flags[TRI_TRI_COLLISION_BUFFER_OVERFLOW_INDEX] = 1 + intersection_count = intersection_count + 1 + + triangle_intersecting_triangles_count[tri_index] = intersection_count + + +@wp.struct +class TriMeshCollisionInfo: + vertex_colliding_triangles: wp.array(dtype=wp.int32) + vertex_colliding_triangles_offsets: wp.array(dtype=wp.int32) + vertex_colliding_triangles_buffer_sizes: wp.array(dtype=wp.int32) + vertex_colliding_triangles_count: wp.array(dtype=wp.int32) + vertex_colliding_triangles_min_dist: wp.array(dtype=float) + + triangle_colliding_vertices: wp.array(dtype=wp.int32) + triangle_colliding_vertices_offsets: wp.array(dtype=wp.int32) + triangle_colliding_vertices_buffer_sizes: wp.array(dtype=wp.int32) + triangle_colliding_vertices_count: wp.array(dtype=wp.int32) + triangle_colliding_vertices_min_dist: wp.array(dtype=float) + + edge_colliding_edges: wp.array(dtype=wp.int32) + edge_colliding_edges_offsets: wp.array(dtype=wp.int32) + edge_colliding_edges_buffer_sizes: wp.array(dtype=wp.int32) + edge_colliding_edges_count: wp.array(dtype=wp.int32) + edge_colliding_edges_min_dist: wp.array(dtype=float) + + +@wp.func +def get_vertex_colliding_triangles_count(col_info: TriMeshCollisionInfo, v: int): + return wp.min(col_info.vertex_colliding_triangles_count[v], col_info.vertex_colliding_triangles_buffer_sizes[v]) + + +@wp.func +def get_vertex_colliding_triangles(col_info: TriMeshCollisionInfo, v: int, i_collision: int): + offset = col_info.vertex_colliding_triangles_offsets[v] + return col_info.vertex_colliding_triangles[offset + i_collision] + + +@wp.func +def get_triangle_colliding_vertices_count(col_info: TriMeshCollisionInfo, tri: int): + return wp.min( + col_info.triangle_colliding_vertices_count[tri], col_info.triangle_colliding_vertices_buffer_sizes[tri] + ) + + +@wp.func +def get_triangle_colliding_vertices(col_info: TriMeshCollisionInfo, tri: int, i_collision: int): + offset = col_info.triangle_colliding_vertices_offsets[tri] + return col_info.triangle_colliding_vertices[offset + i_collision] + + +@wp.func +def get_edge_colliding_edges_count(col_info: TriMeshCollisionInfo, e: int): + return wp.min(col_info.edge_colliding_edges_count[e], col_info.edge_colliding_edges_buffer_sizes[e]) + + +@wp.func +def get_edge_colliding_edges(col_info: TriMeshCollisionInfo, e: int, i_collision: int): + offset = col_info.edge_colliding_edges_offsets[e] + return col_info.edge_colliding_edges[offset + i_collision] + + class TriMeshCollisionDetector: def __init__( self, model: Model, + vertex_positions=None, vertex_collision_buffer_pre_alloc=8, vertex_collision_buffer_max_alloc=256, triangle_collision_buffer_pre_alloc=16, triangle_collision_buffer_max_alloc=256, edge_collision_buffer_pre_alloc=8, edge_collision_buffer_max_alloc=256, + triangle_triangle_collision_buffer_pre_alloc=8, + triangle_triangle_collision_buffer_max_alloc=256, edge_edge_parallel_epsilon=1e-5, ): self.model = model + self.vertex_positions = model.particle_q if vertex_positions is None else vertex_positions self.device = model.device self.vertex_collision_buffer_pre_alloc = vertex_collision_buffer_pre_alloc self.vertex_collision_buffer_max_alloc = vertex_collision_buffer_max_alloc @@ -1955,6 +2124,8 @@ def __init__( self.triangle_collision_buffer_max_alloc = triangle_collision_buffer_max_alloc self.edge_collision_buffer_pre_alloc = edge_collision_buffer_pre_alloc self.edge_collision_buffer_max_alloc = edge_collision_buffer_max_alloc + self.triangle_triangle_collision_buffer_pre_alloc = triangle_triangle_collision_buffer_pre_alloc + self.triangle_triangle_collision_buffer_max_alloc = triangle_triangle_collision_buffer_max_alloc self.edge_edge_parallel_epsilon = edge_edge_parallel_epsilon @@ -2021,14 +2192,44 @@ def __init__( self.upper_bounds_edges = wp.array(shape=(model.edge_count,), dtype=wp.vec3, device=model.device) wp.launch( kernel=compute_edge_aabbs, - inputs=[model.particle_q, model.edge_indices, self.lower_bounds_edges, self.upper_bounds_edges], + inputs=[self.vertex_positions, model.edge_indices, self.lower_bounds_edges, self.upper_bounds_edges], dim=model.edge_count, device=model.device, ) self.bvh_edges = wp.Bvh(self.lower_bounds_edges, self.upper_bounds_edges) - self.resize_flags = wp.zeros(shape=(3,), dtype=wp.int32, device=self.device) + self.resize_flags = wp.zeros(shape=(4,), dtype=wp.int32, device=self.device) + + self.collision_info = self.get_collision_data() + + # data for triangle-triangle intersection; they will only be initialized on demand, as triangle-triangle intersection is not needed for simulation + self.triangle_intersecting_triangles = None + self.triangle_intersecting_triangles_count = None + self.triangle_intersecting_triangles_offsets = None + + def get_collision_data(self): + collision_info = TriMeshCollisionInfo() + + collision_info.vertex_colliding_triangles = self.vertex_colliding_triangles + collision_info.vertex_colliding_triangles_offsets = self.vertex_colliding_triangles_offsets + collision_info.vertex_colliding_triangles_buffer_sizes = self.vertex_colliding_triangles_buffer_sizes + collision_info.vertex_colliding_triangles_count = self.vertex_colliding_triangles_count + collision_info.vertex_colliding_triangles_min_dist = self.vertex_colliding_triangles_min_dist + + collision_info.triangle_colliding_vertices = self.triangle_colliding_vertices + collision_info.triangle_colliding_vertices_offsets = self.triangle_colliding_vertices_offsets + collision_info.triangle_colliding_vertices_buffer_sizes = self.triangle_colliding_vertices_buffer_sizes + collision_info.triangle_colliding_vertices_count = self.triangle_colliding_vertices_count + collision_info.triangle_colliding_vertices_min_dist = self.triangle_colliding_vertices_min_dist + + collision_info.edge_colliding_edges = self.edge_colliding_edges + collision_info.edge_colliding_edges_offsets = self.edge_colliding_edges_offsets + collision_info.edge_colliding_edges_buffer_sizes = self.edge_colliding_edges_buffer_sizes + collision_info.edge_colliding_edges_count = self.edge_colliding_edges_count + collision_info.edge_colliding_edges_min_dist = self.edge_colliding_edges_min_dist + + return collision_info def compute_collision_buffer_offsets( self, buffer_sizes: wp.array(dtype=wp.int32), offsets: wp.array(dtype=wp.int32) @@ -2040,8 +2241,14 @@ def compute_collision_buffer_offsets( offsets.assign(offsets_np) - def refit(self): - self.refit_triangles() + def refit(self, new_pos=None): + if new_pos is not None: + self.vertex_positions = new_pos + # this will automatically apply refit + self.mesh_tris.points = new_pos + else: + self.refit_triangles() + self.refit_edges() def refit_triangles(self): @@ -2050,7 +2257,7 @@ def refit_triangles(self): def refit_edges(self): wp.launch( kernel=compute_edge_aabbs, - inputs=[self.model.particle_q, self.model.edge_indices, self.lower_bounds_edges, self.upper_bounds_edges], + inputs=[self.vertex_positions, self.model.edge_indices, self.lower_bounds_edges, self.upper_bounds_edges], dim=self.model.edge_count, device=self.model.device, ) @@ -2076,7 +2283,7 @@ def vertex_triangle_collision_detection(self, query_radius): inputs=[ query_radius, self.mesh_tris.id, - self.model.particle_q, + self.vertex_positions, self.model.tri_indices, self.vertex_colliding_triangles_offsets, self.vertex_colliding_triangles_buffer_sizes, @@ -2098,11 +2305,11 @@ def vertex_triangle_collision_detection(self, query_radius): def edge_edge_collision_detection(self, query_radius): wp.launch( - kernel=edge_edge_collision_detection_kernel, + kernel=edge_colliding_edges_detection_kernel, inputs=[ query_radius, self.bvh_edges.id, - self.model.particle_q, + self.vertex_positions, self.model.edge_indices, self.edge_colliding_edges_offsets, self.edge_colliding_edges_buffer_sizes, @@ -2117,3 +2324,40 @@ def edge_edge_collision_detection(self, query_radius): dim=self.model.edge_count, device=self.model.device, ) + + def triangle_triangle_intersection_detection(self): + if self.triangle_intersecting_triangles is None: + self.triangle_intersecting_triangles = wp.zeros( + shape=(self.model.tri_count * self.triangle_triangle_collision_buffer_pre_alloc,), + dtype=wp.int32, + device=self.device, + ) + + if self.triangle_intersecting_triangles_count is None: + self.triangle_intersecting_triangles_count = wp.array( + shape=(self.model.tri_count,), dtype=wp.int32, device=self.device + ) + + if self.triangle_intersecting_triangles_offsets is None: + buffer_sizes = np.full((self.model.tri_count,), self.triangle_triangle_collision_buffer_pre_alloc) + offsets = np.zeros((self.model.tri_count + 1,), dtype=np.int32) + offsets[1:] = np.cumsum(buffer_sizes) + + self.triangle_intersecting_triangles_offsets = wp.array(offsets, dtype=wp.int32, device=self.device) + + wp.launch( + kernel=triangle_triangle_collision_detection_kernel, + inputs=[ + self.mesh_tris.id, + self.vertex_positions, + self.model.tri_indices, + self.triangle_intersecting_triangles_offsets, + ], + outputs=[ + self.triangle_intersecting_triangles, + self.triangle_intersecting_triangles_count, + self.resize_flags, + ], + dim=self.model.tri_count, + device=self.model.device, + ) diff --git a/warp/sim/integrator_vbd.py b/warp/sim/integrator_vbd.py index 2bd60d9f0..2c5695449 100644 --- a/warp/sim/integrator_vbd.py +++ b/warp/sim/integrator_vbd.py @@ -9,9 +9,29 @@ import warp as wp from ..types import float32, matrix +from .collide import ( + TriMeshCollisionDetector, + TriMeshCollisionInfo, + get_edge_colliding_edges, + get_edge_colliding_edges_count, + get_triangle_colliding_vertices, + get_triangle_colliding_vertices_count, + get_vertex_colliding_triangles, + get_vertex_colliding_triangles_count, + triangle_closest_point, +) from .integrator import Integrator from .model import PARTICLE_FLAG_ACTIVE, Control, Model, ModelShapeMaterials, State +VBD_DEBUG_PRINTING_OPTIONS = { + # "elasticity_force_hessian", + # "contact_force_hessian", + # "overall_force_hessian", + # "inertia_force_hessian", + # "connectivity", + # "contact_info", +} + class mat66(matrix(shape=(6, 6), dtype=float32)): pass @@ -54,23 +74,23 @@ def to(self, device): @wp.func -def get_vertex_num_adjacent_edges(vertex: wp.int32, adjacency: ForceElementAdjacencyInfo): +def get_vertex_num_adjacent_edges(adjacency: ForceElementAdjacencyInfo, vertex: wp.int32): return (adjacency.v_adj_edges_offsets[vertex + 1] - adjacency.v_adj_edges_offsets[vertex]) >> 1 @wp.func -def get_vertex_adjacent_edge_id_order(vertex: wp.int32, edge: wp.int32, adjacency: ForceElementAdjacencyInfo): +def get_vertex_adjacent_edge_id_order(adjacency: ForceElementAdjacencyInfo, vertex: wp.int32, edge: wp.int32): offset = adjacency.v_adj_edges_offsets[vertex] return adjacency.v_adj_edges[offset + edge * 2], adjacency.v_adj_edges[offset + edge * 2 + 1] @wp.func -def get_vertex_num_adjacent_faces(vertex: wp.int32, adjacency: ForceElementAdjacencyInfo): +def get_vertex_num_adjacent_faces(adjacency: ForceElementAdjacencyInfo, vertex: wp.int32): return (adjacency.v_adj_faces_offsets[vertex + 1] - adjacency.v_adj_faces_offsets[vertex]) >> 1 @wp.func -def get_vertex_adjacent_face_id_order(vertex: wp.int32, face: wp.int32, adjacency: ForceElementAdjacencyInfo): +def get_vertex_adjacent_face_id_order(adjacency: ForceElementAdjacencyInfo, vertex: wp.int32, face: wp.int32): offset = adjacency.v_adj_faces_offsets[vertex] return adjacency.v_adj_faces[offset + face * 2], adjacency.v_adj_faces[offset + face * 2 + 1] @@ -83,9 +103,9 @@ def _test_compute_force_element_adjacency( ): wp.printf("num vertices: %d\n", adjacency.v_adj_edges_offsets.shape[0] - 1) for vertex in range(adjacency.v_adj_edges_offsets.shape[0] - 1): - num_adj_edges = get_vertex_num_adjacent_edges(vertex, adjacency) + num_adj_edges = get_vertex_num_adjacent_edges(adjacency, vertex) for i_bd in range(num_adj_edges): - bd_id, v_order = get_vertex_adjacent_edge_id_order(vertex, i_bd, adjacency) + bd_id, v_order = get_vertex_adjacent_edge_id_order(adjacency, vertex, i_bd) if edge_indices[bd_id, v_order] != vertex: print("Error!!!") @@ -93,10 +113,14 @@ def _test_compute_force_element_adjacency( wp.printf("--iBd: %d | ", i_bd) wp.printf("edge id: %d | v_order: %d\n", bd_id, v_order) - num_adj_faces = get_vertex_num_adjacent_faces(vertex, adjacency) + num_adj_faces = get_vertex_num_adjacent_faces(adjacency, vertex) for i_face in range(num_adj_faces): - face, v_order = get_vertex_adjacent_face_id_order(vertex, i_face, adjacency) + face, v_order = get_vertex_adjacent_face_id_order( + adjacency, + vertex, + i_face, + ) if face_indices[face, v_order] != vertex: print("Error!!!") @@ -200,16 +224,6 @@ def evaluate_stvk_force_hessian( F = calculate_triangle_deformation_gradient(face, tri_indices, pos, tri_pose) G = green_strain(F) - # wp.printf("face: %d, \nF12:\n, %f %f\n, %f %f\n, %f %f\nG:\n%f %f\n, %f %f\n", - # face, - # F[0, 0], F[0, 1], - # F[1, 0], F[1, 1], - # F[2, 0], F[2, 1], - # - # G[0, 0], G[0, 1], - # G[1, 0], G[1, 1], - # ) - S = 2.0 * mu * G + lmbd * (G[0, 0] + G[1, 1]) * wp.identity(n=2, dtype=float) F12 = -area * F * S * wp.transpose(tri_pose) @@ -323,8 +337,8 @@ def evaluate_stvk_force_hessian( @wp.func def evaluate_ground_contact_force_hessian( - vertex_pos: wp.vec3, - vertex_prev_pos: wp.vec3, + particle_pos: wp.vec3, + particle_prev_pos: wp.vec3, particle_radius: float, ground_normal: wp.vec3, ground_level: float, @@ -333,14 +347,14 @@ def evaluate_ground_contact_force_hessian( friction_epsilon: float, dt: float, ): - penetration_depth = -(wp.dot(ground_normal, vertex_pos) + ground_level - particle_radius) + penetration_depth = -(wp.dot(ground_normal, particle_pos) + ground_level - particle_radius) if penetration_depth > 0: ground_contact_force_norm = penetration_depth * soft_contact_ke ground_contact_force = ground_normal * ground_contact_force_norm ground_contact_hessian = soft_contact_ke * wp.outer(ground_normal, ground_normal) - dx = vertex_pos - vertex_prev_pos + dx = particle_pos - particle_prev_pos # friction e0, e1 = build_orthonormal_basis(ground_normal) @@ -438,10 +452,221 @@ def evaluate_body_particle_contact( return body_contact_force, body_contact_hessian +@wp.func +def evaluate_self_contact_force_norm(dis: float, collision_radius: float, k: float): + # Adjust distance and calculate penetration depth + + penetration_depth = collision_radius - dis + + # Initialize outputs + dEdD = wp.float32(0.0) + d2E_dDdD = wp.float32(0.0) + + # C2 continuity calculation + tau = collision_radius * 0.5 + if tau > dis > 1e-5: + k2 = 0.5 * tau * tau * k + dEdD = -k2 / dis + d2E_dDdD = k2 / (dis * dis) + else: + dEdD = -k * penetration_depth + d2E_dDdD = k + + return dEdD, d2E_dDdD + + +@wp.func +def evaluate_edge_edge_contact( + v: int, + v_order: int, + e1: int, + e2: int, + pos: wp.array(dtype=wp.vec3), + pos_prev: wp.array(dtype=wp.vec3), + edge_indices: wp.array(dtype=wp.int32, ndim=2), + collision_radius: float, + collision_stiffness: float, + friction_coefficient: float, + friction_epsilon: float, + dt: float, + edge_edge_parallel_epsilon: float, +): + r""" + Returns the edge-edge contact force and hessian, including the friction force. + Args: + v: + v_order: \in {0, 1, 2, 3}, 0, 1 is vertex 0, 1 of e1, 2,3 is vertex 0, 1 of e2 + e0 + e1 + pos + edge_indices + collision_radius + collision_stiffness + dt + """ + e1_v1 = edge_indices[e1, 2] + e1_v2 = edge_indices[e1, 3] + + e1_v1_pos = pos[e1_v1] + e1_v2_pos = pos[e1_v2] + + e2_v1 = edge_indices[e2, 2] + e2_v2 = edge_indices[e2, 3] + + e2_v1_pos = pos[e2_v1] + e2_v2_pos = pos[e2_v2] + + st = wp.closest_point_edge_edge(e1_v1_pos, e1_v2_pos, e2_v1_pos, e2_v2_pos, edge_edge_parallel_epsilon) + s = st[0] + t = st[1] + e1_vec = e1_v2_pos - e1_v1_pos + e2_vec = e2_v2_pos - e2_v1_pos + c1 = e1_v1_pos + e1_vec * s + c2 = e2_v1_pos + e2_vec * t + + # c1, c2, s, t = closest_point_edge_edge_2(e1_v1_pos, e1_v2_pos, e2_v1_pos, e2_v2_pos) + + diff = c1 - c2 + dis = st[2] + collision_normal = diff / dis + + if dis < collision_radius: + bs = wp.vec4(1.0 - s, s, -1.0 + t, -t) + v_bary = bs[v_order] + + dEdD, d2E_dDdD = evaluate_self_contact_force_norm(dis, collision_radius, collision_stiffness) + + collision_force = -dEdD * v_bary * collision_normal + collision_hessian = d2E_dDdD * v_bary * v_bary * wp.outer(collision_normal, collision_normal) + + # friction + c1_prev = pos_prev[e1_v1] + (pos_prev[e1_v2] - pos_prev[e1_v1]) * s + c2_prev = pos_prev[e2_v1] + (pos_prev[e2_v2] - pos_prev[e2_v1]) * t + + dx = (c1 - c1_prev) - (c2 - c2_prev) + e1_vec_normalized = wp.normalize(e1_vec) + axis_2 = wp.normalize(wp.cross(e1_vec_normalized, collision_normal)) + + T = mat32( + e1_vec_normalized[0], + axis_2[0], + e1_vec_normalized[1], + axis_2[1], + e1_vec_normalized[2], + axis_2[2], + ) + + u = wp.transpose(T) * dx + eps_U = friction_epsilon * dt + + # fmt: off + if wp.static("contact_force_hessian" in VBD_DEBUG_PRINTING_OPTIONS): + wp.printf( + " collision force:\n %f %f %f,\n collision hessian:\n %f %f %f,\n %f %f %f,\n %f %f %f\n", + collision_force[0], collision_force[1], collision_force[2], collision_hessian[0, 0], collision_hessian[0, 1], collision_hessian[0, 2], collision_hessian[1, 0], collision_hessian[1, 1], collision_hessian[1, 2], collision_hessian[2, 0], collision_hessian[2, 1], collision_hessian[2, 2], + ) + # fmt: on + + friction_force, friction_hessian = compute_friction(friction_coefficient, -dEdD, T, u, eps_U) + friction_force = friction_force * v_bary + friction_hessian = friction_hessian * v_bary * v_bary + + # fmt: off + if wp.static("contact_force_hessian" in VBD_DEBUG_PRINTING_OPTIONS): + wp.printf( + " friction force:\n %f %f %f,\n friction hessian:\n %f %f %f,\n %f %f %f,\n %f %f %f\n", + friction_force[0], friction_force[1], friction_force[2], friction_hessian[0, 0], friction_hessian[0, 1], friction_hessian[0, 2], friction_hessian[1, 0], friction_hessian[1, 1], friction_hessian[1, 2], friction_hessian[2, 0], friction_hessian[2, 1], friction_hessian[2, 2], + ) + # fmt: on + + collision_force = collision_force + friction_force + collision_hessian = collision_hessian + friction_hessian + else: + collision_force = wp.vec3(0.0, 0.0, 0.0) + collision_hessian = wp.mat33(0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0) + + return collision_force, collision_hessian + + +@wp.func +def evaluate_vertex_triangle_collision_force_hessian( + v: int, + v_order: int, + tri: int, + pos: wp.array(dtype=wp.vec3), + pos_prev: wp.array(dtype=wp.vec3), + tri_indices: wp.array(dtype=wp.int32, ndim=2), + collision_radius: float, + collision_stiffness: float, + friction_coefficient: float, + friction_epsilon: float, + dt: float, +): + a = pos[tri_indices[tri, 0]] + b = pos[tri_indices[tri, 1]] + c = pos[tri_indices[tri, 2]] + + p = pos[v] + + closest_p, bary, feature_type = triangle_closest_point(a, b, c, p) + + diff = p - closest_p + dis = wp.length(diff) + collision_normal = diff / dis + + if dis < collision_radius: + bs = wp.vec4(-bary[0], -bary[1], -bary[2], 1.0) + v_bary = bs[v_order] + + dEdD, d2E_dDdD = evaluate_self_contact_force_norm(dis, collision_radius, collision_stiffness) + + collision_force = -dEdD * v_bary * collision_normal + collision_hessian = d2E_dDdD * v_bary * v_bary * wp.outer(collision_normal, collision_normal) + + # friction force + dx_v = p - pos_prev[v] + + closest_p_prev = ( + bary[0] * pos_prev[tri_indices[tri, 0]] + + bary[1] * pos_prev[tri_indices[tri, 1]] + + bary[2] * pos_prev[tri_indices[tri, 2]] + ) + + dx = dx_v - (closest_p - closest_p_prev) + + e0 = wp.normalize(b - a) + e1 = wp.normalize(wp.cross(e0, collision_normal)) + + T = mat32(e0[0], e1[0], e0[1], e1[1], e0[2], e1[2]) + + u = wp.transpose(T) * dx + eps_U = friction_epsilon * dt + + friction_force, friction_hessian = compute_friction(friction_coefficient, -dEdD, T, u, eps_U) + + # fmt: off + if wp.static("contact_force_hessian" in VBD_DEBUG_PRINTING_OPTIONS): + wp.printf( + "v: %d dEdD: %f\nnormal force: %f %f %f\nfriction force: %f %f %f\n", + v, + dEdD, + collision_force[0], collision_force[1], collision_force[2], friction_force[0], friction_force[1], friction_force[2], + ) + # fmt: on + + collision_force = collision_force + v_bary * friction_force + collision_hessian = collision_hessian + v_bary * v_bary * friction_hessian + else: + collision_force = wp.vec3(0.0, 0.0, 0.0) + collision_hessian = wp.mat33(0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0) + + return collision_force, collision_hessian + + @wp.func def compute_friction(mu: float, normal_contact_force: float, T: mat32, u: wp.vec2, eps_u: float): """ - Returns the friction force and hessian. + Returns the 1D friction force and hessian. Args: mu: Friction coefficient. normal_contact_force: normal contact force. @@ -496,7 +721,126 @@ def forward_step( @wp.kernel -def VBD_solve_trimesh( +def forward_step_penetration_free( + dt: float, + gravity: wp.vec3, + prev_pos: wp.array(dtype=wp.vec3), + pos: wp.array(dtype=wp.vec3), + vel: wp.array(dtype=wp.vec3), + inv_mass: wp.array(dtype=float), + external_force: wp.array(dtype=wp.vec3), + particle_flags: wp.array(dtype=wp.uint32), + pos_prev_collision_detection: wp.array(dtype=wp.vec3), + particle_conservative_bounds: wp.array(dtype=float), + inertia: wp.array(dtype=wp.vec3), +): + particle_index = wp.tid() + + prev_pos[particle_index] = pos[particle_index] + if not particle_flags[particle_index] & PARTICLE_FLAG_ACTIVE: + inertia[particle_index] = prev_pos[particle_index] + return + vel_new = vel[particle_index] + (gravity + external_force[particle_index] * inv_mass[particle_index]) * dt + pos_inertia = pos[particle_index] + vel_new * dt + inertia[particle_index] = pos_inertia + + pos[particle_index] = apply_conservative_bound_truncation( + particle_index, pos_inertia, pos_prev_collision_detection, particle_conservative_bounds + ) + + +@wp.kernel +def compute_particle_conservative_bound( + # inputs + conservative_bound_relaxation: float, + collision_query_radius: float, + adjacency: ForceElementAdjacencyInfo, + collision_info: TriMeshCollisionInfo, + # outputs + particle_conservative_bounds: wp.array(dtype=float), +): + particle_index = wp.tid() + min_dist = wp.min(collision_query_radius, collision_info.vertex_colliding_triangles_min_dist[particle_index]) + + # bound from neighbor triangles + for i_adj_tri in range( + get_vertex_num_adjacent_faces( + adjacency, + particle_index, + ) + ): + tri_index, vertex_order = get_vertex_adjacent_face_id_order( + adjacency, + particle_index, + i_adj_tri, + ) + min_dist = wp.min(min_dist, collision_info.triangle_colliding_vertices_min_dist[tri_index]) + + # bound from neighbor edges + for i_adj_edge in range( + get_vertex_num_adjacent_edges( + adjacency, + particle_index, + ) + ): + nei_edge_index, vertex_order_on_edge = get_vertex_adjacent_edge_id_order( + adjacency, + particle_index, + i_adj_edge, + ) + # vertex is on the edge; otherwise it only effects the bending energy + if vertex_order_on_edge == 2 or vertex_order_on_edge == 3: + # collisions of neighbor edges + min_dist = wp.min(min_dist, collision_info.edge_colliding_edges_min_dist[nei_edge_index]) + + particle_conservative_bounds[particle_index] = conservative_bound_relaxation * min_dist + + +@wp.kernel +def validate_conservative_bound( + pos: wp.array(dtype=wp.vec3), + pos_prev_collision_detection: wp.array(dtype=wp.vec3), + particle_conservative_bounds: wp.array(dtype=float), +): + v_index = wp.tid() + + displacement = wp.length(pos[v_index] - pos_prev_collision_detection[v_index]) + + if displacement > particle_conservative_bounds[v_index] * 1.01 and displacement > 1e-5: + # wp.expect_eq(displacement <= particle_conservative_bounds[v_index] * 1.01, True) + wp.printf( + "Vertex %d has moved by %f exceeded the limit of %f\n", + v_index, + displacement, + particle_conservative_bounds[v_index], + ) + + +@wp.func +def apply_conservative_bound_truncation( + v_index: wp.int32, + pos_new: wp.vec3, + pos_prev_collision_detection: wp.array(dtype=wp.vec3), + particle_conservative_bounds: wp.array(dtype=float), +): + particle_pos_prev_collision_detection = pos_prev_collision_detection[v_index] + accumulated_displacement = pos_new - particle_pos_prev_collision_detection + conservative_bound = particle_conservative_bounds[v_index] + + accumulated_displacement_norm = wp.length(accumulated_displacement) + if accumulated_displacement_norm > conservative_bound and conservative_bound > 1e-5: + accumulated_displacement_norm_truncated = conservative_bound + accumulated_displacement = accumulated_displacement * ( + accumulated_displacement_norm_truncated / accumulated_displacement_norm + ) + + return particle_pos_prev_collision_detection + accumulated_displacement + else: + return pos_new + + +@wp.kernel +def VBD_solve_trimesh_no_self_contact( dt: float, particle_ids_in_color: wp.array(dtype=wp.int32), prev_pos: wp.array(dtype=wp.vec3), @@ -538,7 +882,6 @@ def VBD_solve_trimesh( tid = wp.tid() particle_index = particle_ids_in_color[tid] - # wp.printf("vId: %d\n", particle) if not particle_flags[particle_index] & PARTICLE_FLAG_ACTIVE: return @@ -553,12 +896,24 @@ def VBD_solve_trimesh( h = mass[particle_index] * dt_sqr_reciprocal * wp.identity(n=3, dtype=float) # elastic force and hessian - for i_adj_tri in range(get_vertex_num_adjacent_faces(particle_index, adjacency)): - # wp.printf("particle: %d | num_adj_faces: %d | ", particle, get_particle_num_adjacent_faces(particle, adjacency)) - tri_id, particle_order = get_vertex_adjacent_face_id_order(particle_index, i_adj_tri, adjacency) - - # wp.printf("i_face: %d | face id: %d | v_order: %d | ", i_adj_tri, tri_id, particle_order) - # wp.printf("face: %d %d %d\n", tri_indices[tri_id, 0], tri_indices[tri_id, 1], tri_indices[tri_id, 2], ) + for i_adj_tri in range(get_vertex_num_adjacent_faces(adjacency, particle_index)): + tri_id, particle_order = get_vertex_adjacent_face_id_order(adjacency, particle_index, i_adj_tri) + + # fmt: off + if wp.static("connectivity" in VBD_DEBUG_PRINTING_OPTIONS): + wp.printf( + "particle: %d | num_adj_faces: %d | ", + particle_index, + get_vertex_num_adjacent_faces(particle_index, adjacency), + ) + wp.printf("i_face: %d | face id: %d | v_order: %d | ", i_adj_tri, tri_id, particle_order) + wp.printf( + "face: %d %d %d\n", + tri_indices[tri_id, 0], + tri_indices[tri_id, 1], + tri_indices[tri_id, 2], + ) + # fmt: on f_tri, h_tri = evaluate_stvk_force_hessian( tri_id, @@ -580,13 +935,16 @@ def VBD_solve_trimesh( f = f + f_tri + f_d h = h + h_tri + h_d - # wp.printf("particle: %d, i_adj_tri: %d, particle_order: %d, \nforce:\n %f %f %f, \nhessian:, \n%f %f %f, \n%f %f %f, \n%f %f %f\n", - # particle, i_adj_tri, particle_order, - # f[0], f[1], f[2], - # h[0, 0], h[0, 1], h[0, 2], - # h[1, 0], h[1, 1], h[1, 2], - # h[2, 0], h[2, 1], h[2, 2], - # ) + # fmt: off + if wp.static("elasticity_force_hessian" in VBD_DEBUG_PRINTING_OPTIONS): + wp.printf( + "particle: %d, i_adj_tri: %d, particle_order: %d, \nforce:\n %f %f %f, \nhessian:, \n%f %f %f, \n%f %f %f, \n%f %f %f\n", + particle_index, + i_adj_tri, + particle_order, + f[0], f[1], f[2], h[0, 0], h[0, 1], h[0, 2], h[1, 0], h[1, 1], h[1, 2], h[2, 0], h[2, 1], h[2, 2], + ) + # fmt: on # body-particle contact particle_contact_count = min(body_particle_contact_count[particle_index], body_particle_contact_buffer_pre_alloc) @@ -687,6 +1045,297 @@ def convert_body_particle_contact_data_kernel( body_particle_contact_buffer[offset + contact_counter] = contact_index +@wp.kernel +def VBD_solve_trimesh_with_self_contact_penetration_free( + dt: float, + particle_ids_in_color: wp.array(dtype=wp.int32), + pos_prev: wp.array(dtype=wp.vec3), + pos: wp.array(dtype=wp.vec3), + pos_new: wp.array(dtype=wp.vec3), + vel: wp.array(dtype=wp.vec3), + mass: wp.array(dtype=float), + inertia: wp.array(dtype=wp.vec3), + particle_flags: wp.array(dtype=wp.uint32), + tri_indices: wp.array(dtype=wp.int32, ndim=2), + tri_poses: wp.array(dtype=wp.mat22), + tri_materials: wp.array(dtype=float, ndim=2), + tri_areas: wp.array(dtype=float), + edge_indices: wp.array(dtype=wp.int32, ndim=2), + adjacency: ForceElementAdjacencyInfo, + # contact info + # self contact + collision_info: TriMeshCollisionInfo, + collision_radius: float, + soft_contact_ke: float, + friction_mu: float, + friction_epsilon: float, + pos_prev_collision_detection: wp.array(dtype=wp.vec3), + particle_conservative_bounds: wp.array(dtype=float), + edge_edge_parallel_epsilon: float, + # body-particle contact + particle_radius: wp.array(dtype=float), + body_particle_contact_buffer_pre_alloc: int, + body_particle_contact_buffer: wp.array(dtype=int), + body_particle_contact_count: wp.array(dtype=int), + shape_materials: ModelShapeMaterials, + shape_body: wp.array(dtype=int), + body_q: wp.array(dtype=wp.transform), + body_qd: wp.array(dtype=wp.spatial_vector), + body_com: wp.array(dtype=wp.vec3), + contact_shape: wp.array(dtype=int), + contact_body_pos: wp.array(dtype=wp.vec3), + contact_body_vel: wp.array(dtype=wp.vec3), + contact_normal: wp.array(dtype=wp.vec3), + # ground-particle contact + has_ground: bool, + ground: wp.array(dtype=float), +): + t_id = wp.tid() + + particle_index = particle_ids_in_color[t_id] + particle_pos = pos[particle_index] + particle_prev_pos = pos_prev[particle_index] + + if not particle_flags[particle_index] & PARTICLE_FLAG_ACTIVE: + return + + dt_sqr_reciprocal = 1.0 / (dt * dt) + + # inertia force and hessian + f = mass[particle_index] * (inertia[particle_index] - pos[particle_index]) * (dt_sqr_reciprocal) + h = mass[particle_index] * dt_sqr_reciprocal * wp.identity(n=3, dtype=float) + + # fmt: off + if wp.static("inertia_force_hessian" in VBD_DEBUG_PRINTING_OPTIONS): + wp.printf( + "particle: %d after accumulate inertia\nforce:\n %f %f %f, \nhessian:, \n%f %f %f, \n%f %f %f, \n%f %f %f\n", + particle_index, + f[0], f[1], f[2], h[0, 0], h[0, 1], h[0, 2], h[1, 0], h[1, 1], h[1, 2], h[2, 0], h[2, 1], h[2, 2], + ) + + # v-side of the v-f collision force + if wp.static("contact_info" in VBD_DEBUG_PRINTING_OPTIONS): + wp.printf("Has %d colliding triangles\n", get_vertex_colliding_triangles_count(collision_info, particle_index)) + for i_v_collision in range(get_vertex_colliding_triangles_count(collision_info, particle_index)): + colliding_t = get_vertex_colliding_triangles(collision_info, particle_index, i_v_collision) + if wp.static("contact_info" in VBD_DEBUG_PRINTING_OPTIONS): + wp.printf( + "vertex %d is colliding with triangle: %d-(%d, %d, %d)", + particle_index, + colliding_t, + tri_indices[colliding_t, 0], + tri_indices[colliding_t, 1], + tri_indices[colliding_t, 2], + ) + # fmt: on + + collision_force, collision_hessian = evaluate_vertex_triangle_collision_force_hessian( + particle_index, + 3, + colliding_t, + pos, + pos_prev, + tri_indices, + collision_radius, + soft_contact_ke, + friction_mu, + friction_epsilon, + dt, + ) + f = f + collision_force + h = h + collision_hessian + + # fmt: off + if wp.static("contact_force_hessian" in VBD_DEBUG_PRINTING_OPTIONS): + wp.printf( + "vertex: %d collision %d:\nforce:\n %f %f %f, \nhessian:, \n%f %f %f, \n%f %f %f, \n%f %f %f\n", + particle_index, + i_v_collision, + collision_force[0], collision_force[1], collision_force[2], collision_hessian[0, 0], collision_hessian[0, 1], collision_hessian[0, 2], collision_hessian[1, 0], collision_hessian[1, 1], collision_hessian[1, 2], collision_hessian[2, 0], collision_hessian[2, 1], collision_hessian[2, 2], + ) + # fmt: on + + # elastic force and hessian + for i_adj_tri in range(get_vertex_num_adjacent_faces(adjacency, particle_index)): + tri_index, vertex_order = get_vertex_adjacent_face_id_order(adjacency, particle_index, i_adj_tri) + + # fmt: off + if wp.static("connectivity" in VBD_DEBUG_PRINTING_OPTIONS): + wp.printf( + "particle: %d | num_adj_faces: %d | ", + particle_index, + get_vertex_num_adjacent_faces(particle_index, adjacency), + ) + wp.printf("i_face: %d | face id: %d | v_order: %d | ", i_adj_tri, tri_index, vertex_order) + wp.printf( + "face: %d %d %d\n", + tri_indices[tri_index, 0], + tri_indices[tri_index, 1], + tri_indices[tri_index, 2], + ) + # fmt: on + + f_tri, h_tri = evaluate_stvk_force_hessian( + tri_index, + vertex_order, + pos, + tri_indices, + tri_poses[tri_index], + tri_areas[tri_index], + tri_materials[tri_index, 0], + tri_materials[tri_index, 1], + tri_materials[tri_index, 2], + ) + # compute damping + k_d = tri_materials[tri_index, 2] + h_d = h_tri * (k_d / dt) + + f_d = h_d * (pos_prev[particle_index] - pos[particle_index]) + + f = f + f_tri + f_d + h = h + h_tri + h_d + + # t-side of vt-collision from the neighbor triangles + # fmt: off + if wp.static("contact_info" in VBD_DEBUG_PRINTING_OPTIONS): + wp.printf( + "Nei triangle %d has %d colliding vertice\n", + tri_index, + get_triangle_colliding_vertices_count(collision_info, tri_index), + ) + # fmt: on + for i_t_collision in range(get_triangle_colliding_vertices_count(collision_info, tri_index)): + colliding_v = get_triangle_colliding_vertices(collision_info, tri_index, i_t_collision) + + collision_force, collision_hessian = evaluate_vertex_triangle_collision_force_hessian( + colliding_v, + vertex_order, + tri_index, + pos, + pos_prev, + tri_indices, + collision_radius, + soft_contact_ke, + friction_mu, + friction_epsilon, + dt, + ) + + f = f + collision_force + h = h + collision_hessian + + # edge-edge collision force and hessian + for i_adj_edge in range(get_vertex_num_adjacent_edges(adjacency, particle_index)): + nei_edge_index, vertex_order_on_edge = get_vertex_adjacent_edge_id_order(adjacency, particle_index, i_adj_edge) + # vertex is on the edge; otherwise it only effects the bending energy n + if vertex_order_on_edge == 2 or vertex_order_on_edge == 3: + # collisions of neighbor triangles + if wp.static("contact_info" in VBD_DEBUG_PRINTING_OPTIONS): + wp.printf( + "Nei edge %d has %d colliding edge\n", + nei_edge_index, + get_edge_colliding_edges_count(collision_info, nei_edge_index), + ) + for i_e_collision in range(get_edge_colliding_edges_count(collision_info, nei_edge_index)): + colliding_e = get_edge_colliding_edges(collision_info, nei_edge_index, i_e_collision) + + collision_force, collision_hessian = evaluate_edge_edge_contact( + particle_index, + vertex_order_on_edge - 2, + nei_edge_index, + colliding_e, + pos, + pos_prev, + edge_indices, + collision_radius, + soft_contact_ke, + friction_mu, + friction_epsilon, + dt, + edge_edge_parallel_epsilon, + ) + f = f + collision_force + h = h + collision_hessian + + # fmt: off + if wp.static("contact_force_hessian" in VBD_DEBUG_PRINTING_OPTIONS): + wp.printf( + "vertex: %d edge %d collision %d:\nforce:\n %f %f %f, \nhessian:, \n%f %f %f, \n%f %f %f, \n%f %f %f\n", + particle_index, + nei_edge_index, + i_e_collision, + collision_force[0], collision_force[1], collision_force[2], collision_hessian[0, 0], collision_hessian[0, 1], collision_hessian[0, 2], collision_hessian[1, 0], collision_hessian[1, 1], collision_hessian[1, 2], collision_hessian[2, 0], collision_hessian[2, 1], collision_hessian[2, 2], + ) + # fmt: on + + # body-particle contact + particle_contact_count = min(body_particle_contact_count[particle_index], body_particle_contact_buffer_pre_alloc) + + offset = body_particle_contact_buffer_pre_alloc * particle_index + for contact_counter in range(particle_contact_count): + # the index to access body-particle data, which is size-variable and only contains active contact + contact_index = body_particle_contact_buffer[offset + contact_counter] + + body_contact_force, body_contact_hessian = evaluate_body_particle_contact( + particle_index, + particle_pos, + particle_prev_pos, + contact_index, + soft_contact_ke, + friction_mu, + friction_epsilon, + particle_radius, + shape_materials, + shape_body, + body_q, + body_qd, + body_com, + contact_shape, + contact_body_pos, + contact_body_vel, + contact_normal, + dt, + ) + + f = f + body_contact_force + h = h + body_contact_hessian + + if has_ground: + ground_normal = wp.vec3(ground[0], ground[1], ground[2]) + ground_level = ground[3] + ground_contact_force, ground_contact_hessian = evaluate_ground_contact_force_hessian( + particle_pos, + particle_prev_pos, + particle_radius[particle_index], + ground_normal, + ground_level, + soft_contact_ke, + friction_mu, + friction_epsilon, + dt, + ) + + f = f + ground_contact_force + h = h + ground_contact_hessian + + # fmt: off + if wp.static("overall_force_hessian" in VBD_DEBUG_PRINTING_OPTIONS): + wp.printf( + "vertex: %d final\noverall force:\n %f %f %f, \noverall hessian:, \n%f %f %f, \n%f %f %f, \n%f %f %f\n", + particle_index, + f[0], f[1], f[2], h[0, 0], h[0, 1], h[0, 2], h[1, 0], h[1, 1], h[1, 2], h[2, 0], h[2, 1], h[2, 2], + ) + # fmt: on + + if abs(wp.determinant(h)) > 1e-5: + h_inv = wp.inverse(h) + particle_pos_new = pos[particle_index] + h_inv * f + + pos_new[particle_index] = apply_conservative_bound_truncation( + particle_index, particle_pos_new, pos_prev_collision_detection, particle_conservative_bounds + ) + + class VBDIntegrator(Integrator): """An implicit integrator using Vertex Block Descent (VBD) for cloth simulation. @@ -718,8 +1367,14 @@ def __init__( self, model: Model, iterations=10, - body_particle_contact_buffer_pre_alloc=4, + handle_self_contact=False, + penetration_free_conservative_bound_relaxation=0.42, friction_epsilon=1e-2, + body_particle_contact_buffer_pre_alloc=4, + vertex_collision_buffer_pre_alloc=32, + edge_collision_buffer_pre_alloc=64, + triangle_collision_buffer_pre_alloc=32, + edge_edge_parallel_epsilon=1e-5, ): self.device = model.device self.model = model @@ -731,6 +1386,7 @@ def __init__( self.adjacency = self.compute_force_element_adjacency(model).to(self.device) + # data for body-particle collision self.body_particle_contact_buffer_pre_alloc = body_particle_contact_buffer_pre_alloc self.body_particle_contact_buffer = wp.zeros( (self.body_particle_contact_buffer_pre_alloc * model.particle_count,), @@ -738,6 +1394,28 @@ def __init__( device=self.device, ) self.body_particle_contact_count = wp.zeros((model.particle_count,), dtype=wp.int32, device=self.device) + + self.handle_self_contact = handle_self_contact + + if handle_self_contact: + if self.model.soft_contact_margin < self.model.soft_contact_radius: + raise ValueError( + "model.soft_contact_margin is smaller than self.model.soft_contact_radius, this will result in missing contacts and cause instability. \n" + "It is advisable to make model.soft_contact_margin 1.5~2 times larger than self.model.soft_contact_radius." + ) + + self.conservative_bound_relaxation = penetration_free_conservative_bound_relaxation + self.pos_prev_collision_detection = wp.zeros_like(model.particle_q, device=self.device) + self.particle_conservative_bounds = wp.full((model.particle_count,), dtype=float, device=self.device) + + self.trimesh_collision_detector = TriMeshCollisionDetector( + self.model, + vertex_collision_buffer_pre_alloc=vertex_collision_buffer_pre_alloc, + edge_collision_buffer_pre_alloc=edge_collision_buffer_pre_alloc, + triangle_collision_buffer_pre_alloc=triangle_collision_buffer_pre_alloc, + edge_edge_parallel_epsilon=edge_edge_parallel_epsilon, + ) + self.friction_epsilon = friction_epsilon if len(self.model.particle_coloring) == 0: @@ -837,6 +1515,14 @@ def simulate(self, model: Model, state_in: State, state_out: State, dt: float, c if model is not self.model: raise ValueError("model must be the one used to initialize VBDIntegrator") + if self.handle_self_contact: + self.simulate_one_step_with_collisions_penetration_free(model, state_in, state_out, dt, control) + else: + self.simulate_one_step_no_self_contact(model, state_in, state_out, dt, control) + + def simulate_one_step_no_self_contact( + self, model: Model, state_in: State, state_out: State, dt: float, control: Control = None + ): self.convert_body_particle_contact_data() wp.launch( @@ -859,7 +1545,7 @@ def simulate(self, model: Model, state_in: State, state_out: State, dt: float, c for _iter in range(self.iterations): for color_counter in range(len(self.model.particle_coloring)): wp.launch( - kernel=VBD_solve_trimesh, + kernel=VBD_solve_trimesh_no_self_contact, inputs=[ dt, self.model.particle_coloring[color_counter], @@ -914,6 +1600,120 @@ def simulate(self, model: Model, state_in: State, state_out: State, dt: float, c device=self.device, ) + def simulate_one_step_with_collisions_penetration_free( + self, model: Model, state_in: State, state_out: State, dt: float, control: Control = None + ): + self.convert_body_particle_contact_data() + # collision detection before initialization to compute conservative bounds for initialization + self.collision_detection_penetration_free(state_in, dt) + + wp.launch( + kernel=forward_step_penetration_free, + inputs=[ + dt, + model.gravity, + self.particle_q_prev, + state_in.particle_q, + state_in.particle_qd, + self.model.particle_inv_mass, + state_in.particle_f, + self.model.particle_flags, + self.pos_prev_collision_detection, + self.particle_conservative_bounds, + self.inertia, + ], + dim=self.model.particle_count, + device=self.device, + ) + + # after initialization, we do another collision detection to update the bounds + self.collision_detection_penetration_free(state_in, dt) + + for _iter in range(self.iterations): + for i_color in range(len(self.model.particle_coloring)): + wp.launch( + kernel=VBD_solve_trimesh_with_self_contact_penetration_free, + inputs=[ + dt, + self.model.particle_coloring[i_color], + self.particle_q_prev, + state_in.particle_q, + state_out.particle_q, + state_in.particle_qd, + self.model.particle_mass, + self.inertia, + self.model.particle_flags, + self.model.tri_indices, + self.model.tri_poses, + self.model.tri_materials, + self.model.tri_areas, + self.model.edge_indices, + self.adjacency, + # self-contact + self.trimesh_collision_detector.collision_info, + self.model.soft_contact_radius, + self.model.soft_contact_ke, + self.model.soft_contact_mu, + self.friction_epsilon, + self.pos_prev_collision_detection, + self.particle_conservative_bounds, + self.trimesh_collision_detector.edge_edge_parallel_epsilon, + # body-particle contact + self.model.particle_radius, + self.body_particle_contact_buffer_pre_alloc, + self.body_particle_contact_buffer, + self.body_particle_contact_count, + self.model.shape_materials, + self.model.shape_body, + self.model.body_q, + self.model.body_qd, + self.model.body_com, + self.model.soft_contact_shape, + self.model.soft_contact_body_pos, + self.model.soft_contact_body_vel, + self.model.soft_contact_normal, + self.model.ground, + self.model.ground_plane, + ], + dim=self.model.particle_coloring[i_color].shape[0], + device=self.device, + ) + + wp.launch( + kernel=VBD_copy_particle_positions_back, + inputs=[self.model.particle_coloring[i_color], state_in.particle_q, state_out.particle_q], + dim=self.model.particle_coloring[i_color].size, + device=self.device, + ) + + wp.launch( + kernel=update_velocity, + inputs=[dt, self.particle_q_prev, state_out.particle_q, state_out.particle_qd], + dim=self.model.particle_count, + device=self.device, + ) + + def collision_detection_penetration_free(self, current_state, dt): + self.trimesh_collision_detector.refit(current_state.particle_q) + self.trimesh_collision_detector.vertex_triangle_collision_detection(self.model.soft_contact_margin) + self.trimesh_collision_detector.edge_edge_collision_detection(self.model.soft_contact_margin) + + self.pos_prev_collision_detection.assign(current_state.particle_q) + wp.launch( + kernel=compute_particle_conservative_bound, + inputs=[ + self.conservative_bound_relaxation, + self.model.soft_contact_margin, + self.adjacency, + self.trimesh_collision_detector.collision_info, + ], + outputs=[ + self.particle_conservative_bounds, + ], + dim=self.model.particle_count, + device=self.device, + ) + def convert_body_particle_contact_data(self): self.body_particle_contact_count.zero_() diff --git a/warp/sim/model.py b/warp/sim/model.py index 7889faf57..da6803885 100644 --- a/warp/sim/model.py +++ b/warp/sim/model.py @@ -591,6 +591,7 @@ class Model: joint_attach_ke (float): Joint attachment force stiffness (used by :class:`SemiImplicitIntegrator`) joint_attach_kd (float): Joint attachment force damping (used by :class:`SemiImplicitIntegrator`) + soft_contact_radius (float): Contact radius used for self-collisions in the VBD integrator. soft_contact_margin (float): Contact margin for generation of soft contacts soft_contact_ke (float): Stiffness of soft contacts (used by the Euler integrators) soft_contact_kd (float): Damping of soft contacts (used by the Euler integrators) @@ -761,6 +762,7 @@ def __init__(self, device=None): self.joint_attach_ke = 1.0e3 self.joint_attach_kd = 1.0e2 + self.soft_contact_radius = 0.2 self.soft_contact_margin = 0.2 self.soft_contact_ke = 1.0e3 self.soft_contact_kd = 10.0 diff --git a/warp/tests/test_collision.py b/warp/tests/test_collision.py index 2fc10aa1d..6f6e2b74e 100644 --- a/warp/tests/test_collision.py +++ b/warp/tests/test_collision.py @@ -430,8 +430,8 @@ def run_edge_edge_test(self): def set_points_fixed(self, model, fixed_particles): if len(fixed_particles): flags = model.particle_flags.numpy() - for fixed_v_id in fixed_particles: - flags[fixed_v_id] = wp.uint32(int(flags[fixed_v_id]) & ~int(PARTICLE_FLAG_ACTIVE)) + for fixed_vertex_id in fixed_particles: + flags[fixed_vertex_id] = wp.uint32(int(flags[fixed_vertex_id]) & ~int(PARTICLE_FLAG_ACTIVE)) model.particle_flags = wp.array(flags, device=model.device)