warp-lang 1.6.2__py3-none-macosx_10_13_universal2.whl → 1.7.0__py3-none-macosx_10_13_universal2.whl

warp/sim/import_urdf.py

@@ -20,7 +20,6 @@ from typing import Union
 import numpy as np
 
 import warp as wp
-import warp.sim
 from warp.sim.model import Mesh
 
 
@@ -46,6 +45,7 @@ def parse_urdf(
     joint_limit_lower=-1e6,
     joint_limit_upper=1e6,
     scale=1.0,
+    hide_visuals=False,
     parse_visuals_as_colliders=False,
     force_show_colliders=False,
     enable_self_collisions=True,
@@ -79,6 +79,7 @@ def parse_urdf(
         joint_limit_lower (float): The default lower joint limit if not specified in the URDF.
         joint_limit_upper (float): The default upper joint limit if not specified in the URDF.
         scale (float): The scaling factor to apply to the imported mechanism.
+        hide_visuals (bool): If True, hide visual shapes.
         parse_visuals_as_colliders (bool): If True, the geometry defined under the `<visual>` tags is used for collision handling instead of the `<collision>` geometries.
         force_show_colliders (bool): If True, the collision shapes are always shown, even if there are visual shapes.
         enable_self_collisions (bool): If True, self-collisions are enabled.
@@ -173,6 +174,22 @@ def parse_urdf(
                 )
                 shapes.append(s)
 
+            for capsule in geo.findall("capsule"):
+                s = builder.add_shape_capsule(
+                    body=link,
+                    pos=wp.vec3(tf.p),
+                    rot=wp.quat(tf.q),
+                    radius=float(capsule.get("radius") or "1") * scale,
+                    half_height=float(capsule.get("height") or "1") * 0.5 * scale,
+                    density=density,
+                    up_axis=2,  # capsules in URDF are aligned with z-axis
+                    is_visible=visible,
+                    has_ground_collision=not just_visual,
+                    has_shape_collision=not just_visual,
+                    **contact_vars,
+                )
+                shapes.append(s)
+
             for mesh in geo.findall("mesh"):
                 filename = mesh.get("filename")
                 if filename is None:
@@ -219,15 +236,15 @@ def parse_urdf(
                 scaling = np.array([float(x) * scale for x in scaling.split()])
                 if hasattr(m, "geometry"):
                     # multiple meshes are contained in a scene
-                    for geom in m.geometry.values():
-                        geom_vertices = np.array(geom.vertices, dtype=np.float32) * scaling
-                        geom_faces = np.array(geom.faces.flatten(), dtype=np.int32)
-                        geom_mesh = Mesh(geom_vertices, geom_faces)
+                    for m_geom in m.geometry.values():
+                        m_vertices = np.array(m_geom.vertices, dtype=np.float32) * scaling
+                        m_faces = np.array(m_geom.faces.flatten(), dtype=np.int32)
+                        m_mesh = Mesh(m_vertices, m_faces)
                         s = builder.add_shape_mesh(
                             body=link,
                             pos=wp.vec3(tf.p),
                             rot=wp.quat(tf.q),
-                            mesh=geom_mesh,
+                            mesh=m_mesh,
                             density=density,
                             is_visible=visible,
                             has_ground_collision=not just_visual,
@@ -278,7 +295,7 @@ def parse_urdf(
         if parse_visuals_as_colliders:
             colliders = visuals
         else:
-            s = parse_shapes(link, visuals, density=0.0, just_visual=True)
+            s = parse_shapes(link, visuals, density=0.0, just_visual=True, visible=not hide_visuals)
             visual_shapes.extend(s)
 
         show_colliders = force_show_colliders
@@ -309,10 +326,13 @@ def parse_urdf(
             I_m = rot @ wp.mat33(I_m)
             m = float(inertial.find("mass").get("value") or "0")
             builder.body_mass[link] = m
-            builder.body_inv_mass[link] = 1.0 / m
+            builder.body_inv_mass[link] = 1.0 / m if m > 0.0 else 0.0
             builder.body_com[link] = com
             builder.body_inertia[link] = I_m
-            builder.body_inv_inertia[link] = wp.inverse(I_m)
+            if any(x for x in I_m):
+                builder.body_inv_inertia[link] = wp.inverse(I_m)
+            else:
+                builder.body_inv_inertia[link] = I_m
         if m == 0.0 and ensure_nonstatic_links:
             # set the mass to something nonzero to ensure the body is dynamic
             m = static_link_mass
@@ -369,7 +389,7 @@ def parse_urdf(
 
     # topological sorting of joints because the FK solver will resolve body transforms
     # in joint order and needs the parent link transform to be resolved before the child
-    visited = {name: False for name in link_index.keys()}
+    visited = dict.fromkeys(link_index.keys(), False)
     sorted_joints = []
 
     # depth-first search
@@ -520,7 +540,7 @@ def parse_urdf(
 
             builder.add_joint_d6(
                 linear_axes=[
-                    warp.sim.JointAxis(
+                    wp.sim.JointAxis(
                         u,
                         limit_lower=lower * scale,
                         limit_upper=upper * scale,
@@ -530,7 +550,7 @@ def parse_urdf(
                         target_kd=joint_damping,
                         mode=joint_mode,
                     ),
-                    warp.sim.JointAxis(
+                    wp.sim.JointAxis(
                         v,
                         limit_lower=lower * scale,
                         limit_upper=upper * scale,

warp/sim/integrator_euler.py

@@ -559,7 +559,7 @@ def eval_tetrahedra(
     v20 = v2 - v0
     v30 = v3 - v0
 
-    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
@@ -574,7 +574,7 @@ def eval_tetrahedra(
 
     # F = Xs*Xm^-1
     F = Ds * Dm
-    dFdt = wp.mat33(v10, v20, v30) * Dm
+    dFdt = wp.matrix_from_cols(v10, v20, v30) * Dm
 
     col1 = wp.vec3(F[0, 0], F[1, 0], F[2, 0])
     col2 = wp.vec3(F[0, 1], F[1, 1], F[2, 1])
@@ -660,9 +660,9 @@ def eval_tetrahedra(
 
     # alpha = 1.0
 
-    # I = wp.mat33(wp.vec3(1.0, 0.0, 0.0),
-    #              wp.vec3(0.0, 1.0, 0.0),
-    #              wp.vec3(0.0, 0.0, 1.0))
+    # I = wp.matrix_from_cols(wp.vec3(1.0, 0.0, 0.0),
+    #                         wp.vec3(0.0, 1.0, 0.0),
+    #                         wp.vec3(0.0, 0.0, 1.0))
 
     # P = (F + wp.transpose(F) + I*(0.0-2.0))*k_mu
     # H = P * wp.transpose(Dm)

warp/sim/integrator_featherstone.py

@@ -128,7 +128,7 @@ def spatial_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.skew(p) @ R
 
     T = spatial_adjoint(R, S)
@@ -298,7 +298,7 @@ def jcalc_motion(
     if type == wp.sim.JOINT_UNIVERSAL:
         axis_0 = joint_axis[axis_start + 0]
         axis_1 = joint_axis[axis_start + 1]
-        q_off = wp.quat_from_matrix(wp.mat33(axis_0, axis_1, wp.cross(axis_0, axis_1)))
+        q_off = wp.quat_from_matrix(wp.matrix_from_cols(axis_0, axis_1, wp.cross(axis_0, axis_1)))
         local_0 = wp.quat_rotate(q_off, wp.vec3(1.0, 0.0, 0.0))
         local_1 = wp.quat_rotate(q_off, wp.vec3(0.0, 1.0, 0.0))
 
@@ -319,7 +319,7 @@ def jcalc_motion(
         axis_0 = joint_axis[axis_start + 0]
         axis_1 = joint_axis[axis_start + 1]
         axis_2 = joint_axis[axis_start + 2]
-        q_off = wp.quat_from_matrix(wp.mat33(axis_0, axis_1, axis_2))
+        q_off = wp.quat_from_matrix(wp.matrix_from_cols(axis_0, axis_1, axis_2))
         local_0 = wp.quat_rotate(q_off, wp.vec3(1.0, 0.0, 0.0))
         local_1 = wp.quat_rotate(q_off, wp.vec3(0.0, 1.0, 0.0))
         local_2 = wp.quat_rotate(q_off, wp.vec3(0.0, 0.0, 1.0))
@@ -2066,13 +2066,6 @@ class FeatherstoneIntegrator(Integrator):
                         ],
                         device=model.device,
                     )
-                    # if wp.context.runtime.tape:
-                    #     wp.context.runtime.tape.record_func(
-                    #         backward=lambda: adj_matmul(
-                    #             a, b, c, a.grad, b.grad, c.grad, d.grad, alpha, beta, allow_tf32x3_arith, device
-                    #         ),
-                    #         arrays=[a, b, c, d],
-                    #     )
                     # print("joint_qdd:")
                     # print(state_aug.joint_qdd.numpy())
                     # print("\n\n")

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

@@ -595,7 +595,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 +627,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 +691,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 +766,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 +837,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
@@ -716,6 +733,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
 
@@ -878,7 +896,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 +1223,7 @@ class ModelBuilder:
         # edges (bending)
         self.edge_indices = []
         self.edge_rest_angle = []
+        self.edge_rest_length = []
         self.edge_bending_properties = []
 
         # tetrahedra
@@ -1384,7 +1403,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 +1541,7 @@ class ModelBuilder:
             "particle_radius",
             "particle_flags",
             "edge_rest_angle",
+            "edge_rest_length",
             "edge_bending_properties",
             "spring_rest_length",
             "spring_stiffness",
@@ -3757,11 +3781,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 +3800,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 +3832,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 +3864,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:
@@ -4628,6 +4656,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/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)