warp-lang 1.6.2__py3-none-win_amd64.whl → 1.7.1__py3-none-win_amd64.whl

warp/sim/integrator_vbd.py

@@ -346,6 +346,172 @@ def evaluate_stvk_force_hessian(
     return f, h
 
 
+@wp.func
+def mat_vec_cross_from_3_basis(e1: wp.vec3, e2: wp.vec3, e3: wp.vec3, a: wp.vec3):
+    e1_cross_a = wp.cross(e1, a)
+    e2_cross_a = wp.cross(e2, a)
+    e3_cross_a = wp.cross(e3, a)
+
+    return wp.mat33(
+        e1_cross_a[0],
+        e2_cross_a[0],
+        e3_cross_a[0],
+        e1_cross_a[1],
+        e2_cross_a[1],
+        e3_cross_a[1],
+        e1_cross_a[2],
+        e2_cross_a[2],
+        e3_cross_a[2],
+    )
+
+
+@wp.func
+def mat_vec_cross(mat: wp.mat33, a: wp.vec3):
+    e1 = wp.vec3(mat[0, 0], mat[1, 0], mat[2, 0])
+    e2 = wp.vec3(mat[0, 1], mat[1, 1], mat[2, 1])
+    e3 = wp.vec3(mat[0, 2], mat[1, 2], mat[2, 2])
+
+    return mat_vec_cross_from_3_basis(e1, e2, e3, a)
+
+
+@wp.func
+def evaluate_dihedral_angle_based_bending_force_hessian(
+    bending_index: int,
+    v_order: int,
+    pos: wp.array(dtype=wp.vec3),
+    pos_prev: wp.array(dtype=wp.vec3),
+    edge_indices: wp.array(dtype=wp.int32, ndim=2),
+    edge_rest_angle: wp.array(dtype=float),
+    edge_rest_length: wp.array(dtype=float),
+    stiffness: float,
+    damping: float,
+    dt: float,
+):
+    if edge_indices[bending_index, 0] == -1 or edge_indices[bending_index, 1] == -1:
+        return wp.vec3(0.0), wp.mat33(0.0)
+
+    x1 = pos[edge_indices[bending_index, 0]]
+    x2 = pos[edge_indices[bending_index, 2]]
+    x3 = pos[edge_indices[bending_index, 3]]
+    x4 = pos[edge_indices[bending_index, 1]]
+
+    e1 = wp.vec3(1.0, 0.0, 0.0)
+    e2 = wp.vec3(0.0, 1.0, 0.0)
+    e3 = wp.vec3(0.0, 0.0, 1.0)
+
+    n1 = wp.cross((x2 - x1), (x3 - x1))
+    n2 = wp.cross((x3 - x4), (x2 - x4))
+    n1_norm = wp.length(n1)
+    n2_norm = wp.length(n2)
+
+    # degenerated bending edge
+    if n1_norm < 1.0e-6 or n2_norm < 1.0e-6:
+        return wp.vec3(0.0), wp.mat33(0.0)
+
+    n1_n = n1 / n1_norm
+    n2_n = n2 / n2_norm
+
+    # avoid the infinite gradient of acos at -1 or 1
+    cos_theta = wp.dot(n1_n, n2_n)
+    if wp.abs(cos_theta) > 0.9999:
+        cos_theta = 0.9999 * wp.sign(cos_theta)
+
+    angle_sign = wp.sign(wp.dot(wp.cross(n2, n1), x3 - x2))
+    theta = wp.acos(cos_theta) * angle_sign
+    rest_angle = edge_rest_angle[bending_index]
+
+    dE_dtheta = stiffness * (theta - rest_angle)
+
+    d_theta_d_cos_theta = angle_sign * (-1.0 / wp.sqrt(1.0 - cos_theta * cos_theta))
+    sin_theta = angle_sign * wp.sqrt(1.0 - cos_theta * cos_theta)
+    one_over_sin_theta = 1.0 / sin_theta
+    d_one_over_sin_theta_d_cos_theta = cos_theta / (sin_theta * sin_theta * sin_theta)
+
+    e_rest_len = edge_rest_length[bending_index]
+
+    if v_order == 0:
+        d_cos_theta_dx1 = 1.0 / n1_norm * (-wp.cross(x3 - x1, n2_n) + wp.cross(x2 - x1, n2_n))
+        d_one_over_sin_theta_dx1 = d_cos_theta_dx1 * d_one_over_sin_theta_d_cos_theta
+
+        d_theta_dx1 = d_theta_d_cos_theta * d_cos_theta_dx1
+        d2_theta_dx1_dx1 = -wp.outer(d_one_over_sin_theta_dx1, d_cos_theta_dx1)
+
+        dE_dx1 = e_rest_len * dE_dtheta * d_theta_d_cos_theta * d_cos_theta_dx1
+
+        d2_E_dx1_dx1 = (
+            e_rest_len * stiffness * (wp.outer(d_theta_dx1, d_theta_dx1) + (theta - rest_angle) * d2_theta_dx1_dx1)
+        )
+
+        bending_force = -dE_dx1
+        bending_hessian = d2_E_dx1_dx1
+    elif v_order == 1:
+        d_cos_theta_dx4 = 1.0 / n2_norm * (-wp.cross(x2 - x4, n1_n) + wp.cross(x3 - x4, n1_n))
+        d_one_over_sin_theta_dx4 = d_cos_theta_dx4 * d_one_over_sin_theta_d_cos_theta
+
+        d_theta_dx4 = d_theta_d_cos_theta * d_cos_theta_dx4
+        d2_theta_dx4_dx4 = -wp.outer(d_one_over_sin_theta_dx4, d_cos_theta_dx4)
+
+        dE_dx4 = e_rest_len * dE_dtheta * d_theta_d_cos_theta * d_cos_theta_dx4
+        d2_E_dx4_dx4 = (
+            e_rest_len * stiffness * (wp.outer(d_theta_dx4, d_theta_dx4) + (theta - rest_angle) * (d2_theta_dx4_dx4))
+        )
+
+        bending_force = -dE_dx4
+        bending_hessian = d2_E_dx4_dx4
+    elif v_order == 2:
+        d_cos_theta_dx2 = 1.0 / n1_norm * wp.cross(x3 - x1, n2_n) - 1.0 / n2_norm * wp.cross(x3 - x4, n1_n)
+        dn1_dx2 = mat_vec_cross_from_3_basis(e1, e2, e3, x3 - x1)
+        dn2_dx2 = -mat_vec_cross_from_3_basis(e1, e2, e3, x3 - x4)
+        d_one_over_sin_theta_dx2 = d_cos_theta_dx2 * d_one_over_sin_theta_d_cos_theta
+        d2_cos_theta_dx2_dx2 = -mat_vec_cross(dn2_dx2, (x3 - x1)) / (n1_norm * n2_norm) + mat_vec_cross(
+            dn1_dx2, x3 - x4
+        ) / (n1_norm * n2_norm)
+
+        d_theta_dx2 = d_theta_d_cos_theta * d_cos_theta_dx2
+        d2_theta_dx2_dx2 = (
+            -wp.outer(d_one_over_sin_theta_dx2, d_cos_theta_dx2) - one_over_sin_theta * d2_cos_theta_dx2_dx2
+        )
+
+        dE_dx2 = e_rest_len * dE_dtheta * d_theta_d_cos_theta * d_cos_theta_dx2
+        d2_E_dx2_dx2 = (
+            e_rest_len * stiffness * (wp.outer(d_theta_dx2, d_theta_dx2) + (theta - rest_angle) * d2_theta_dx2_dx2)
+        )
+
+        bending_force = -dE_dx2
+        bending_hessian = d2_E_dx2_dx2
+    else:
+        d_cos_theta_dx3 = -1.0 / n1_norm * wp.cross(x2 - x1, n2_n) + 1.0 / n2_norm * wp.cross(x2 - x4, n1_n)
+        dn1_dx3 = -mat_vec_cross_from_3_basis(e1, e2, e3, x2 - x1)
+        dn2_dx3 = mat_vec_cross_from_3_basis(e1, e2, e3, x2 - x4)
+        d_one_over_sin_theta_dx3 = d_cos_theta_dx3 * d_one_over_sin_theta_d_cos_theta
+        d2_cos_theta_dx3_dx3 = mat_vec_cross(dn2_dx3, (x2 - x1)) / (n1_norm * n2_norm) - mat_vec_cross(
+            dn1_dx3, x2 - x4
+        ) / (n1_norm * n2_norm)
+
+        d_theta_dx3 = d_theta_d_cos_theta * d_cos_theta_dx3
+        d2_theta_dx3_dx3 = (
+            -wp.outer(d_one_over_sin_theta_dx3, d_cos_theta_dx3) - one_over_sin_theta * d2_cos_theta_dx3_dx3
+        )
+
+        dE_dx3 = e_rest_len * dE_dtheta * d_theta_d_cos_theta * d_cos_theta_dx3
+
+        d2_E_dx3_dx3 = (
+            e_rest_len * stiffness * (wp.outer(d_theta_dx3, d_theta_dx3) + (theta - rest_angle) * d2_theta_dx3_dx3)
+        )
+
+        bending_force = -dE_dx3
+        bending_hessian = d2_E_dx3_dx3
+
+    displacement = pos_prev[edge_indices[bending_index, v_order]] - pos[edge_indices[bending_index, v_order]]
+    h_d = bending_hessian * (damping / dt)
+    f_d = h_d * displacement
+
+    bending_force = bending_force + f_d
+    bending_hessian = bending_hessian + h_d
+
+    return bending_force, bending_hessian
+
+
 @wp.func
 def evaluate_ground_contact_force_hessian(
     particle_pos: wp.vec3,
@@ -866,6 +1032,9 @@ def VBD_solve_trimesh_no_self_contact(
     tri_materials: wp.array(dtype=float, ndim=2),
     tri_areas: wp.array(dtype=float),
     edge_indices: wp.array(dtype=wp.int32, ndim=2),
+    edge_rest_angles: wp.array(dtype=float),
+    edge_rest_length: wp.array(dtype=float),
+    edge_bending_properties: wp.array(dtype=float, ndim=2),
     adjacency: ForceElementAdjacencyInfo,
     # contact info
     #   self contact
@@ -957,6 +1126,24 @@ def VBD_solve_trimesh_no_self_contact(
             )
         # fmt: on
 
+    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)
+        f_edge, h_edge = evaluate_dihedral_angle_based_bending_force_hessian(
+            nei_edge_index,
+            vertex_order_on_edge,
+            pos,
+            prev_pos,
+            edge_indices,
+            edge_rest_angles,
+            edge_rest_length,
+            edge_bending_properties[nei_edge_index, 0],
+            edge_bending_properties[nei_edge_index, 1],
+            dt,
+        )
+
+        f = f + f_edge
+        h = h + h_edge
+
     # body-particle contact
     particle_contact_count = min(body_particle_contact_count[particle_index], body_particle_contact_buffer_pre_alloc)
 
@@ -1072,6 +1259,9 @@ def VBD_solve_trimesh_with_self_contact_penetration_free(
     tri_materials: wp.array(dtype=float, ndim=2),
     tri_areas: wp.array(dtype=float),
     edge_indices: wp.array(dtype=wp.int32, ndim=2),
+    edge_rest_angles: wp.array(dtype=float),
+    edge_rest_length: wp.array(dtype=float),
+    edge_bending_properties: wp.array(dtype=float, ndim=2),
     adjacency: ForceElementAdjacencyInfo,
     # contact info
     #   self contact
@@ -1239,6 +1429,15 @@ def VBD_solve_trimesh_with_self_contact_penetration_free(
     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 edge_bending_properties[nei_edge_index, 0] != 0:
+            f_edge, h_edge = evaluate_dihedral_angle_based_bending_force_hessian(
+                nei_edge_index, vertex_order_on_edge, pos, pos_prev, edge_indices, edge_rest_angles, edge_rest_length,
+                edge_bending_properties[nei_edge_index, 0], edge_bending_properties[nei_edge_index, 1], dt
+            )
+
+            f = f + f_edge
+            h = h + h_edge
+
         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):
@@ -1572,6 +1771,9 @@ class VBDIntegrator(Integrator):
                         self.model.tri_materials,
                         self.model.tri_areas,
                         self.model.edge_indices,
+                        self.model.edge_rest_angle,
+                        self.model.edge_rest_length,
+                        self.model.edge_bending_properties,
                         self.adjacency,
                         self.model.soft_contact_ke,
                         self.model.soft_contact_mu,
@@ -1659,6 +1861,9 @@ class VBDIntegrator(Integrator):
                         self.model.tri_materials,
                         self.model.tri_areas,
                         self.model.edge_indices,
+                        self.model.edge_rest_angle,
+                        self.model.edge_rest_length,
+                        self.model.edge_bending_properties,
                         self.adjacency,
                         #   self-contact
                         self.trimesh_collision_detector.collision_info,
@@ -1783,7 +1988,7 @@ class VBDIntegrator(Integrator):
             vertex_adjacent_edges[buffer_offset_v1 + fill_count_v1 * 2 + 1] = 3
             vertex_adjacent_edges_fill_count[v1] = fill_count_v1 + 1
 
-            o0 = edges_array[edge_id, 2]
+            o0 = edges_array[edge_id, 0]
             if o0 != -1:
                 fill_count_o0 = vertex_adjacent_edges_fill_count[o0]
                 buffer_offset_o0 = vertex_adjacent_edges_offsets[o0]
@@ -1791,7 +1996,7 @@ class VBDIntegrator(Integrator):
                 vertex_adjacent_edges[buffer_offset_o0 + fill_count_o0 * 2 + 1] = 0
                 vertex_adjacent_edges_fill_count[o0] = fill_count_o0 + 1
 
-            o1 = edges_array[edge_id, 3]
+            o1 = edges_array[edge_id, 1]
             if o1 != -1:
                 fill_count_o1 = vertex_adjacent_edges_fill_count[o1]
                 buffer_offset_o1 = vertex_adjacent_edges_offsets[o1]

warp/sim/integrator_xpbd.py

@@ -401,6 +401,9 @@ def solve_springs(
     i = spring_indices[tid * 2 + 0]
     j = spring_indices[tid * 2 + 1]
 
+    if i == -1 or j == -1:
+        return
+
     ke = spring_stiffness[tid]
     kd = spring_damping[tid]
     rest = spring_rest_lengths[tid]
@@ -595,7 +598,7 @@ def solve_tetrahedra(
     x20 = x2 - x0
     x30 = x3 - x0
 
-    Ds = wp.mat33(x10, x20, x30)
+    Ds = wp.matrix_from_cols(x10, x20, x30)
     Dm = rest_matrix[tid]
     inv_QT = wp.transpose(Dm)
 
@@ -627,7 +630,7 @@ def solve_tetrahedra(
         elif term == 1:
             # volume conservation
             C = wp.determinant(F) - 1.0
-            dC = wp.mat33(wp.cross(f2, f3), wp.cross(f3, f1), wp.cross(f1, f2))
+            dC = wp.matrix_from_cols(wp.cross(f2, f3), wp.cross(f3, f1), wp.cross(f1, f2))
             compliance = volume_compliance
 
         if C != 0.0:
@@ -691,7 +694,7 @@ def solve_tetrahedra(
     # J = wp.determinant(F)
 
     # C_vol = J - alpha
-    # # dCdx = wp.mat33(wp.cross(f2, f3), wp.cross(f3, f1), wp.cross(f1, f2))*wp.transpose(Dm)
+    # # dCdx = wp.matrix_from_cols(wp.cross(f2, f3), wp.cross(f3, f1), wp.cross(f1, f2))*wp.transpose(Dm)
 
     # # grad1 = wp.vec3(dCdx[0,0], dCdx[1,0], dCdx[2,0])
     # # grad2 = wp.vec3(dCdx[0,1], dCdx[1,1], dCdx[2,1])
@@ -766,7 +769,7 @@ def solve_tetrahedra2(
     x20 = x2 - x0
     x30 = x3 - x0
 
-    Ds = wp.mat33(x10, x20, x30)
+    Ds = wp.matrix_from_cols(x10, x20, x30)
     Dm = pose[tid]
 
     inv_rest_volume = wp.determinant(Dm) * 6.0
@@ -837,7 +840,7 @@ def solve_tetrahedra2(
     J = wp.determinant(F)
 
     C_vol = J - alpha
-    # dCdx = wp.mat33(wp.cross(f2, f3), wp.cross(f3, f1), wp.cross(f1, f2))*wp.transpose(Dm)
+    # dCdx = wp.matrix_from_cols(wp.cross(f2, f3), wp.cross(f3, f1), wp.cross(f1, f2))*wp.transpose(Dm)
 
     # grad1 = wp.vec3(dCdx[0,0], dCdx[1,0], dCdx[2,0])
     # grad2 = wp.vec3(dCdx[0,1], dCdx[1,1], dCdx[2,1])

warp/sim/model.py

@@ -372,9 +372,14 @@ class Control:
     should generally be created using the :func:`Model.control()` function.
     """
 
-    def __init__(self, model: Model):
-        self.model: Model = model
-        """Model to use as a reference for the control inputs."""
+    def __init__(self, model: Model = None):
+        if model:
+            wp.utils.warn(
+                "Passing arguments to Control's __init__ is deprecated\n"
+                "and will be disallowed in a future version. Use Control() without arguments\ninstead.",
+                category=DeprecationWarning,
+                stacklevel=2,
+            )
 
         self.joint_act: Optional[wp.array] = None
         """Array of joint control inputs with shape ``(joint_axis_count,)`` and type ``float``."""
@@ -388,17 +393,28 @@ class Control:
         self.muscle_activations: Optional[wp.array] = None
         """Array of muscle activations with shape ``(muscle_count,)`` and type ``float``."""
 
-    def reset(self) -> None:
-        """Reset the control inputs to their initial state defined in :attr:`model`."""
+    def clear(self) -> None:
+        """Reset the control inputs to zero."""
 
         if self.joint_act is not None:
-            self.joint_act.assign(self.model.joint_act)
+            self.joint_act.zero_()
         if self.tri_activations is not None:
-            self.tri_activations.assign(self.model.tri_activations)
+            self.tri_activations.zero_()
         if self.tet_activations is not None:
-            self.tet_activations.assign(self.model.tet_activations)
+            self.tet_activations.zero_()
         if self.muscle_activations is not None:
-            self.muscle_activations.assign(self.model.muscle_activations)
+            self.muscle_activations.zero_()
+
+    def reset(self) -> None:
+        """Reset the control inputs to zero."""
+
+        wp.utils.warn(
+            "Control.reset() is deprecated and will be removed\nin a future version. Use Control.clear() instead.",
+            category=DeprecationWarning,
+            stacklevel=2,
+        )
+
+        self.clear()
 
 
 def compute_shape_mass(type, scale, src, density, is_solid, thickness):
@@ -543,6 +559,7 @@ class Model:
 
         edge_indices (array): Bending edge indices, shape [edge_count*4], int, each row is [o0, o1, v1, v2], where v1, v2 are on the edge
         edge_rest_angle (array): Bending edge rest angle, shape [edge_count], float
+        edge_rest_length (array): Bending edge rest length, shape [edge_count], float
         edge_bending_properties (array): Bending edge stiffness and damping parameters, shape [edge_count, 2], float
 
         tet_indices (array): Tetrahedral element indices, shape [tet_count*4], int
@@ -616,6 +633,7 @@ class Model:
         soft_contact_body_pos (array), Positional offset of soft contact point in body frame, shape [soft_contact_max], vec3
         soft_contact_body_vel (array), Linear velocity of soft contact point in body frame, shape [soft_contact_max], vec3
         soft_contact_normal (array), Contact surface normal of soft contact point in world space, shape [soft_contact_max], vec3
+        soft_contact_tids (array), Thread indices of the soft contact points, shape [soft_contact_max], int
 
         rigid_contact_max (int): Maximum number of potential rigid body contact points to generate ignoring the `rigid_mesh_contact_max` limit.
         rigid_contact_max_limited (int): Maximum number of potential rigid body contact points to generate respecting the `rigid_mesh_contact_max` limit.
@@ -633,6 +651,12 @@ class Model:
         rigid_contact_thickness (array): Total contact thickness, shape [rigid_contact_max], float
         rigid_contact_shape0 (array): Index of shape 0 per contact, shape [rigid_contact_max], int
         rigid_contact_shape1 (array): Index of shape 1 per contact, shape [rigid_contact_max], int
+        rigid_contact_tids (array): Triangle indices of the contact points, shape [rigid_contact_max], int
+        rigid_contact_pairwise_counter (array): Pairwise counter for contact generation, shape [rigid_contact_max], int
+        rigid_contact_broad_shape0 (array): Broadphase shape index of shape 0 per contact, shape [rigid_contact_max], int
+        rigid_contact_broad_shape1 (array): Broadphase shape index of shape 1 per contact, shape [rigid_contact_max], int
+        rigid_contact_point_id (array): Contact point ID, shape [rigid_contact_max], int
+        rigid_contact_point_limit (array): Contact point limit, shape [rigid_contact_max], int
 
         ground (bool): Whether the ground plane and ground contacts are enabled
         ground_plane (array): Ground plane 3D normal and offset, shape [4], float
@@ -716,6 +740,7 @@ class Model:
 
         self.edge_indices = None
         self.edge_rest_angle = None
+        self.edge_rest_length = None
         self.edge_bending_properties = None
         self.edge_constraint_lambdas = None
 
@@ -787,6 +812,7 @@ class Model:
         self.soft_contact_body_pos = None
         self.soft_contact_body_vel = None
         self.soft_contact_normal = None
+        self.soft_contact_tids = None
 
         self.rigid_contact_max = 0
         self.rigid_contact_max_limited = 0
@@ -804,6 +830,12 @@ class Model:
         self.rigid_contact_thickness = None
         self.rigid_contact_shape0 = None
         self.rigid_contact_shape1 = None
+        self.rigid_contact_tids = None
+        self.rigid_contact_pairwise_counter = None
+        self.rigid_contact_broad_shape0 = None
+        self.rigid_contact_broad_shape1 = None
+        self.rigid_contact_point_id = None
+        self.rigid_contact_point_limit = None
 
         # toggles ground contact for all shapes
         self.ground = True
@@ -878,7 +910,7 @@ class Model:
         Returns:
             Control: The control object
         """
-        c = Control(self)
+        c = Control()
         if requires_grad is None:
             requires_grad = self.requires_grad
         if clone_variables:
@@ -1205,6 +1237,7 @@ class ModelBuilder:
         # edges (bending)
         self.edge_indices = []
         self.edge_rest_angle = []
+        self.edge_rest_length = []
         self.edge_bending_properties = []
 
         # tetrahedra
@@ -1384,7 +1417,11 @@ class ModelBuilder:
         if builder.spring_count:
             self.spring_indices.extend((np.array(builder.spring_indices, dtype=np.int32) + start_particle_idx).tolist())
         if builder.edge_count:
-            self.edge_indices.extend((np.array(builder.edge_indices, dtype=np.int32) + start_particle_idx).tolist())
+            # Update edge indices by adding offset, preserving -1 values
+            edge_indices = np.array(builder.edge_indices, dtype=np.int32)
+            mask = edge_indices != -1
+            edge_indices[mask] += start_particle_idx
+            self.edge_indices.extend(edge_indices.tolist())
         if builder.tri_count:
             self.tri_indices.extend((np.array(builder.tri_indices, dtype=np.int32) + start_particle_idx).tolist())
         if builder.tet_count:
@@ -1518,6 +1555,7 @@ class ModelBuilder:
             "particle_radius",
             "particle_flags",
             "edge_rest_angle",
+            "edge_rest_length",
             "edge_bending_properties",
             "spring_rest_length",
             "spring_stiffness",
@@ -3447,8 +3485,9 @@ class ModelBuilder:
         self.shape_shape_collision.append(has_shape_collision)
 
         (m, c, I) = compute_shape_mass(type, scale, src, density, is_solid, thickness)
+        com_body = wp.transform_point(wp.transform(pos, rot), c)
 
-        self._update_body_mass(body, m, I, pos + c, rot)
+        self._update_body_mass(body, m, I, com_body, rot)
         return shape
 
     # particles
@@ -3757,11 +3796,11 @@ class ModelBuilder:
         edge_kd = edge_kd if edge_kd is not None else self.default_edge_kd
 
         # compute rest angle
+        x3 = self.particle_q[k]
+        x4 = self.particle_q[l]
         if rest is None:
             x1 = self.particle_q[i]
             x2 = self.particle_q[j]
-            x3 = self.particle_q[k]
-            x4 = self.particle_q[l]
 
             n1 = wp.normalize(wp.cross(x3 - x1, x4 - x1))
             n2 = wp.normalize(wp.cross(x4 - x2, x3 - x2))
@@ -3776,6 +3815,7 @@ class ModelBuilder:
 
         self.edge_indices.append((i, j, k, l))
         self.edge_rest_angle.append(rest)
+        self.edge_rest_length.append(wp.length(x4 - x3))
         self.edge_bending_properties.append((edge_ke, edge_kd))
 
     def add_edges(
@@ -3807,6 +3847,8 @@ class ModelBuilder:
             winding: (i, k, l), (j, l, k).
 
         """
+        x3 = np.array(self.particle_q)[k]
+        x4 = np.array(self.particle_q)[l]
         if rest is None:
             # compute rest angle
             x1 = np.array(self.particle_q)[i]
@@ -3837,6 +3879,7 @@ class ModelBuilder:
 
         self.edge_indices.extend(inds.tolist())
         self.edge_rest_angle.extend(rest.tolist())
+        self.edge_rest_length.extend(np.linalg.norm(x4 - x3, axis=1).tolist())
 
         def init_if_none(arr, defaultValue):
             if arr is None:
@@ -3982,15 +4025,16 @@ class ModelBuilder:
             )  # opposite 0, opposite 1, vertex 0, vertex 1
 
             # skip constraints open edges
-            if e.f0 != -1 and e.f1 != -1:
-                spring_indices.add((min(e.o0, e.o1), max(e.o0, e.o1)))
+            spring_indices.add((min(e.v0, e.v1), max(e.v0, e.v1)))
+            if e.f0 != -1:
                 spring_indices.add((min(e.o0, e.v0), max(e.o0, e.v0)))
                 spring_indices.add((min(e.o0, e.v1), max(e.o0, e.v1)))
-
+            if e.f1 != -1:
                 spring_indices.add((min(e.o1, e.v0), max(e.o1, e.v0)))
                 spring_indices.add((min(e.o1, e.v1), max(e.o1, e.v1)))
 
-                spring_indices.add((min(e.v0, e.v1), max(e.v0, e.v1)))
+            if e.f0 != -1 and e.f1 != -1:
+                spring_indices.add((min(e.o0, e.o1), max(e.o0, e.o1)))
 
         if add_springs:
             for i, j in spring_indices:
@@ -4101,14 +4145,15 @@ class ModelBuilder:
         if add_springs:
             spring_indices = set()
             for i, j, k, l in edge_indices:
-                spring_indices.add((min(i, j), max(i, j)))
-                spring_indices.add((min(i, k), max(i, k)))
-                spring_indices.add((min(i, l), max(i, l)))
-
-                spring_indices.add((min(j, k), max(j, k)))
-                spring_indices.add((min(j, l), max(j, l)))
-
                 spring_indices.add((min(k, l), max(k, l)))
+                if i != -1:
+                    spring_indices.add((min(i, k), max(i, k)))
+                    spring_indices.add((min(i, l), max(i, l)))
+                if j != -1:
+                    spring_indices.add((min(j, k), max(j, k)))
+                    spring_indices.add((min(j, l), max(j, l)))
+                if i != -1 and j != -1:
+                    spring_indices.add((min(i, j), max(i, j)))
 
             for i, j in spring_indices:
                 self.add_spring(i, j, spring_ke, spring_kd, control=0.0)
@@ -4628,6 +4673,7 @@ class ModelBuilder:
 
             m.edge_indices = wp.array(self.edge_indices, dtype=wp.int32)
             m.edge_rest_angle = wp.array(self.edge_rest_angle, dtype=wp.float32, requires_grad=requires_grad)
+            m.edge_rest_length = wp.array(self.edge_rest_length, dtype=wp.float32, requires_grad=requires_grad)
             m.edge_bending_properties = wp.array(
                 self.edge_bending_properties, dtype=wp.float32, requires_grad=requires_grad
             )

warp/sim/render.py

@@ -273,6 +273,10 @@ def CreateSimRenderer(renderer):
                             continue
                         tf = joint_tf[i]
                         body = int(joint_parent[i])
+                        if body >= 0 and body < len(self.body_names):
+                            body = self.body_names[body]
+                        else:
+                            body = None
                         # if body == -1:
                         #     continue
                         num_linear_axes = int(joint_axis_dim[i][0])

warp/sim/utils.py

@@ -65,7 +65,7 @@ def quat_decompose(q: wp.quat):
     Decompose a quaternion into a sequence of 3 rotations around x,y',z' respectively, i.e.: q = q_z''q_y'q_x.
     """
 
-    R = wp.mat33(
+    R = wp.matrix_from_cols(
         wp.quat_rotate(q, wp.vec3(1.0, 0.0, 0.0)),
         wp.quat_rotate(q, wp.vec3(0.0, 1.0, 0.0)),
         wp.quat_rotate(q, wp.vec3(0.0, 0.0, 1.0)),
@@ -249,7 +249,7 @@ def transform_inertia(t: wp.transform, I: wp.spatial_matrix):
     r2 = wp.quat_rotate(q, wp.vec3(0.0, 1.0, 0.0))
     r3 = wp.quat_rotate(q, wp.vec3(0.0, 0.0, 1.0))
 
-    R = wp.mat33(r1, r2, r3)
+    R = wp.matrix_from_cols(r1, r2, r3)
     S = wp.mul(wp.skew(p), R)
 
     T = wp.spatial_adjoint(R, S)