warp-lang 1.9.0__py3-none-win_amd64.whl → 1.10.0rc2__py3-none-win_amd64.whl

warp/sim/utils.py

@@ -1,431 +0,0 @@
-# SPDX-FileCopyrightText: Copyright (c) 2022 NVIDIA CORPORATION & AFFILIATES. All rights reserved.
-# SPDX-License-Identifier: Apache-2.0
-#
-# Licensed under the Apache License, Version 2.0 (the "License");
-# you may not use this file except in compliance with the License.
-# You may obtain a copy of the License at
-#
-# http://www.apache.org/licenses/LICENSE-2.0
-#
-# Unless required by applicable law or agreed to in writing, software
-# distributed under the License is distributed on an "AS IS" BASIS,
-# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
-# See the License for the specific language governing permissions and
-# limitations under the License.
-
-from __future__ import annotations
-
-import numpy as np
-
-import warp as wp
-
-
-@wp.func
-def velocity_at_point(qd: wp.spatial_vector, r: wp.vec3):
-    """
-    Returns the velocity of a point relative to the frame with the given spatial velocity.
-
-    Args:
-        qd (spatial_vector): The spatial velocity of the frame.
-        r (vec3): The position of the point relative to the frame.
-
-    Returns:
-        vec3: The velocity of the point.
-    """
-    return wp.cross(wp.spatial_top(qd), r) + wp.spatial_bottom(qd)
-
-
-@wp.func
-def quat_twist(axis: wp.vec3, q: wp.quat):
-    """
-    Returns the twist around an axis.
-    """
-
-    # project imaginary part onto axis
-    a = wp.vec3(q[0], q[1], q[2])
-    proj = wp.dot(a, axis)
-    a = proj * axis
-    # if proj < 0.0:
-    #     # ensure twist points in same direction as axis
-    #     a = -a
-    return wp.normalize(wp.quat(a[0], a[1], a[2], q[3]))
-
-
-@wp.func
-def quat_twist_angle(axis: wp.vec3, q: wp.quat):
-    """
-    Returns the angle of the twist around an axis.
-    """
-    return 2.0 * wp.acos(quat_twist(axis, q)[3])
-
-
-@wp.func
-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.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)),
-    )
-
-    # https://www.sedris.org/wg8home/Documents/WG80485.pdf
-    phi = wp.atan2(R[1, 2], R[2, 2])
-    sinp = -R[0, 2]
-    if wp.abs(sinp) >= 1.0:
-        theta = wp.HALF_PI * wp.sign(sinp)
-    else:
-        theta = wp.asin(-R[0, 2])
-    psi = wp.atan2(R[0, 1], R[0, 0])
-
-    return -wp.vec3(phi, theta, psi)
-
-
-@wp.func
-def quat_to_rpy(q: wp.quat):
-    """
-    Convert a quaternion into Euler angles (roll, pitch, yaw)
-    roll is rotation around x in radians (counterclockwise)
-    pitch is rotation around y in radians (counterclockwise)
-    yaw is rotation around z in radians (counterclockwise)
-    """
-    x = q[0]
-    y = q[1]
-    z = q[2]
-    w = q[3]
-    t0 = 2.0 * (w * x + y * z)
-    t1 = 1.0 - 2.0 * (x * x + y * y)
-    roll_x = wp.atan2(t0, t1)
-
-    t2 = 2.0 * (w * y - z * x)
-    t2 = wp.clamp(t2, -1.0, 1.0)
-    pitch_y = wp.asin(t2)
-
-    t3 = 2.0 * (w * z + x * y)
-    t4 = 1.0 - 2.0 * (y * y + z * z)
-    yaw_z = wp.atan2(t3, t4)
-
-    return wp.vec3(roll_x, pitch_y, yaw_z)
-
-
-@wp.func
-def quat_to_euler(q: wp.quat, i: int, j: int, k: int) -> wp.vec3:
-    """
-    Convert a quaternion into Euler angles.
-
-    :math:`i, j, k` are the indices in :math:`[0, 1, 2]` of the axes to use
-    (:math:`i \\neq j, j \\neq k`).
-
-    Reference: https://journals.plos.org/plosone/article?id=10.1371/journal.pone.0276302
-
-    Args:
-        q (quat): The quaternion to convert
-        i (int): The index of the first axis
-        j (int): The index of the second axis
-        k (int): The index of the third axis
-
-    Returns:
-        vec3: The Euler angles (in radians)
-    """
-    # i, j, k are actually assumed to follow 1-based indexing but
-    # we want to be compatible with quat_from_euler
-    i += 1
-    j += 1
-    k += 1
-    not_proper = True
-    if i == k:
-        not_proper = False
-        k = 6 - i - j  # because i + j + k = 1 + 2 + 3 = 6
-    e = float((i - j) * (j - k) * (k - i)) / 2.0  # Levi-Civita symbol
-    a = q[0]
-    b = q[i]
-    c = q[j]
-    d = q[k] * e
-    if not_proper:
-        a -= q[j]
-        b += q[k] * e
-        c += q[0]
-        d -= q[i]
-    t2 = wp.acos(2.0 * (a * a + b * b) / (a * a + b * b + c * c + d * d) - 1.0)
-    tp = wp.atan2(b, a)
-    tm = wp.atan2(d, c)
-    t1 = 0.0
-    t3 = 0.0
-    if wp.abs(t2) < 1e-6:
-        t3 = 2.0 * tp - t1
-    elif wp.abs(t2 - wp.HALF_PI) < 1e-6:
-        t3 = 2.0 * tm + t1
-    else:
-        t1 = tp - tm
-        t3 = tp + tm
-    if not_proper:
-        t2 -= wp.HALF_PI
-        t3 *= e
-    return wp.vec3(t1, t2, t3)
-
-
-@wp.func
-def quat_from_euler(e: wp.vec3, i: int, j: int, k: int) -> wp.quat:
-    """
-    Convert Euler angles to a quaternion.
-
-    :math:`i, j, k` are the indices in :math:`[0, 1, 2]` of the axes in which the Euler angles are provided
-    (:math:`i \\neq j, j \\neq k`), e.g. (0, 1, 2) for Euler sequence XYZ.
-
-    Args:
-        e (vec3): The Euler angles (in radians)
-        i (int): The index of the first axis
-        j (int): The index of the second axis
-        k (int): The index of the third axis
-
-    Returns:
-        quat: The quaternion
-    """
-    # Half angles
-    half_e = e / 2.0
-
-    # Precompute sines and cosines of half angles
-    cr = wp.cos(half_e[i])
-    sr = wp.sin(half_e[i])
-    cp = wp.cos(half_e[j])
-    sp = wp.sin(half_e[j])
-    cy = wp.cos(half_e[k])
-    sy = wp.sin(half_e[k])
-
-    # Components of the quaternion based on the rotation sequence
-    return wp.quat(
-        (cy * sr * cp - sy * cr * sp),
-        (cy * cr * sp + sy * sr * cp),
-        (sy * cr * cp - cy * sr * sp),
-        (cy * cr * cp + sy * sr * sp),
-    )
-
-
-@wp.func
-def transform_twist(t: wp.transform, x: wp.spatial_vector):
-    # Frank & Park definition 3.20, pg 100
-
-    q = wp.transform_get_rotation(t)
-    p = wp.transform_get_translation(t)
-
-    w = wp.spatial_top(x)
-    v = wp.spatial_bottom(x)
-
-    w = wp.quat_rotate(q, w)
-    v = wp.quat_rotate(q, v) + wp.cross(p, w)
-
-    return wp.spatial_vector(w, v)
-
-
-@wp.func
-def transform_wrench(t: wp.transform, x: wp.spatial_vector):
-    q = wp.transform_get_rotation(t)
-    p = wp.transform_get_translation(t)
-
-    w = wp.spatial_top(x)
-    v = wp.spatial_bottom(x)
-
-    v = wp.quat_rotate(q, v)
-    w = wp.quat_rotate(q, w) + wp.cross(p, v)
-
-    return wp.spatial_vector(w, v)
-
-
-@wp.func
-def transform_inertia(t: wp.transform, I: wp.spatial_matrix):
-    """
-    Computes adj_t^-T*I*adj_t^-1 (tensor change of coordinates).
-    (Frank & Park, section 8.2.3, pg 290)
-    """
-
-    t_inv = wp.transform_inverse(t)
-
-    q = wp.transform_get_rotation(t_inv)
-    p = wp.transform_get_translation(t_inv)
-
-    r1 = wp.quat_rotate(q, wp.vec3(1.0, 0.0, 0.0))
-    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.matrix_from_cols(r1, r2, r3)
-    S = wp.mul(wp.skew(p), R)
-
-    T = wp.spatial_adjoint(R, S)
-
-    return wp.mul(wp.mul(wp.transpose(T), I), T)
-
-
-@wp.func
-def boltzmann(a: float, b: float, alpha: float):
-    e1 = wp.exp(alpha * a)
-    e2 = wp.exp(alpha * b)
-    return (a * e1 + b * e2) / (e1 + e2)
-
-
-@wp.func
-def smooth_max(a: float, b: float, eps: float):
-    d = a - b
-    return 0.5 * (a + b + wp.sqrt(d * d + eps))
-
-
-@wp.func
-def smooth_min(a: float, b: float, eps: float):
-    d = a - b
-    return 0.5 * (a + b - wp.sqrt(d * d + eps))
-
-
-@wp.func
-def leaky_max(a: float, b: float):
-    return smooth_max(a, b, 1e-5)
-
-
-@wp.func
-def leaky_min(a: float, b: float):
-    return smooth_min(a, b, 1e-5)
-
-
-@wp.func
-def vec_min(a: wp.vec3, b: wp.vec3):
-    return wp.vec3(wp.min(a[0], b[0]), wp.min(a[1], b[1]), wp.min(a[2], b[2]))
-
-
-@wp.func
-def vec_max(a: wp.vec3, b: wp.vec3):
-    return wp.vec3(wp.max(a[0], b[0]), wp.max(a[1], b[1]), wp.max(a[2], b[2]))
-
-
-@wp.func
-def vec_leaky_min(a: wp.vec3, b: wp.vec3):
-    return wp.vec3(leaky_min(a[0], b[0]), leaky_min(a[1], b[1]), leaky_min(a[2], b[2]))
-
-
-@wp.func
-def vec_leaky_max(a: wp.vec3, b: wp.vec3):
-    return wp.vec3(leaky_max(a[0], b[0]), leaky_max(a[1], b[1]), leaky_max(a[2], b[2]))
-
-
-@wp.func
-def vec_abs(a: wp.vec3):
-    return wp.vec3(wp.abs(a[0]), wp.abs(a[1]), wp.abs(a[2]))
-
-
-def load_mesh(filename: str, method: str | None = None):
-    """Load a 3D triangular surface mesh from a file.
-
-    Args:
-        filename (str): The path to the 3D model file (obj, and other formats
-          supported by the different methods) to load.
-        method (str): The method to use for loading the mesh (default ``None``).
-          Can be either ``"trimesh"``, ``"meshio"``, ``"pcu"``, or ``"openmesh"``.
-          If ``None``, every method is tried and the first successful mesh import
-          where the number of vertices is greater than 0 is returned.
-
-    Returns:
-        Tuple of (mesh_points, mesh_indices), where mesh_points is a Nx3 numpy array of vertex positions (float32),
-        and mesh_indices is a Mx3 numpy array of vertex indices (int32) for the triangular faces.
-    """
-    import os
-
-    if not os.path.exists(filename):
-        raise FileNotFoundError(f"File not found: {filename}")
-
-    def load_mesh_with_method(method):
-        if method == "meshio":
-            import meshio
-
-            m = meshio.read(filename)
-            mesh_points = np.array(m.points)
-            mesh_indices = np.array(m.cells[0].data, dtype=np.int32)
-        elif method == "openmesh":
-            import openmesh
-
-            m = openmesh.read_trimesh(filename)
-            mesh_points = np.array(m.points())
-            mesh_indices = np.array(m.face_vertex_indices(), dtype=np.int32)
-        elif method == "pcu":
-            import point_cloud_utils as pcu
-
-            mesh_points, mesh_indices = pcu.load_mesh_vf(filename)
-            mesh_indices = mesh_indices.flatten()
-        else:
-            import trimesh
-
-            m = trimesh.load(filename)
-            if hasattr(m, "geometry"):
-                # multiple meshes are contained in a scene; combine to one mesh
-                mesh_points = []
-                mesh_indices = []
-                index_offset = 0
-                for geom in m.geometry.values():
-                    vertices = np.array(geom.vertices, dtype=np.float32)
-                    faces = np.array(geom.faces.flatten(), dtype=np.int32)
-                    mesh_points.append(vertices)
-                    mesh_indices.append(faces + index_offset)
-                    index_offset += len(vertices)
-                mesh_points = np.concatenate(mesh_points, axis=0)
-                mesh_indices = np.concatenate(mesh_indices)
-            else:
-                # a single mesh
-                mesh_points = np.array(m.vertices, dtype=np.float32)
-                mesh_indices = np.array(m.faces.flatten(), dtype=np.int32)
-        return mesh_points, mesh_indices
-
-    if method is None:
-        methods = ["trimesh", "meshio", "pcu", "openmesh"]
-        for method in methods:
-            try:
-                mesh = load_mesh_with_method(method)
-                if mesh is not None and len(mesh[0]) > 0:
-                    return mesh
-            except Exception:
-                pass
-        raise ValueError(f"Failed to load mesh using any of the methods: {methods}")
-    else:
-        mesh = load_mesh_with_method(method)
-        if mesh is None or len(mesh[0]) == 0:
-            raise ValueError(f"Failed to load mesh using method {method}")
-        return mesh
-
-
-def visualize_meshes(
-    meshes: list[tuple[list, list]], num_cols=0, num_rows=0, titles=None, scale_axes=True, show_plot=True
-):
-    # render meshes in a grid with matplotlib
-    import matplotlib.pyplot as plt
-
-    if titles is None:
-        titles = []
-
-    num_cols = min(num_cols, len(meshes))
-    num_rows = min(num_rows, len(meshes))
-    if num_cols and not num_rows:
-        num_rows = int(np.ceil(len(meshes) / num_cols))
-    elif num_rows and not num_cols:
-        num_cols = int(np.ceil(len(meshes) / num_rows))
-    else:
-        num_cols = len(meshes)
-        num_rows = 1
-
-    vertices = [np.array(v).reshape((-1, 3)) for v, _ in meshes]
-    faces = [np.array(f, dtype=np.int32).reshape((-1, 3)) for _, f in meshes]
-    if scale_axes:
-        ranges = np.array([v.max(axis=0) - v.min(axis=0) for v in vertices])
-        max_range = ranges.max()
-        mid_points = np.array([v.max(axis=0) + v.min(axis=0) for v in vertices]) * 0.5
-
-    fig = plt.figure(figsize=(12, 6))
-    for i, (vertices, faces) in enumerate(meshes):
-        ax = fig.add_subplot(num_rows, num_cols, i + 1, projection="3d")
-        if i < len(titles):
-            ax.set_title(titles[i])
-        ax.plot_trisurf(vertices[:, 0], vertices[:, 1], vertices[:, 2], triangles=faces, edgecolor="k")
-        if scale_axes:
-            mid = mid_points[i]
-            ax.set_xlim(mid[0] - max_range, mid[0] + max_range)
-            ax.set_ylim(mid[1] - max_range, mid[1] + max_range)
-            ax.set_zlim(mid[2] - max_range, mid[2] + max_range)
-    if show_plot:
-        plt.show()
-    return fig

warp/tests/sim/disabled_kinematics.py

@@ -1,244 +0,0 @@
-# SPDX-FileCopyrightText: Copyright (c) 2022 NVIDIA CORPORATION & AFFILIATES. All rights reserved.
-# SPDX-License-Identifier: Apache-2.0
-#
-# Licensed under the Apache License, Version 2.0 (the "License");
-# you may not use this file except in compliance with the License.
-# You may obtain a copy of the License at
-#
-# http://www.apache.org/licenses/LICENSE-2.0
-#
-# Unless required by applicable law or agreed to in writing, software
-# distributed under the License is distributed on an "AS IS" BASIS,
-# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
-# See the License for the specific language governing permissions and
-# limitations under the License.
-
-import math
-import unittest
-
-import warp as wp
-import warp.examples
-import warp.sim
-from warp.tests.unittest_utils import *
-
-
-def build_ant(num_envs):
-    builder = wp.sim.ModelBuilder()
-    for i in range(num_envs):
-        wp.sim.parse_mjcf(os.path.join(warp.examples.get_asset_directory(), "nv_ant.xml"), builder, up_axis="y")
-
-        coord_count = 15
-        dof_count = 14
-
-        coord_start = i * coord_count
-        dof_start = i * dof_count
-
-        # base
-        p = [i * 2.0, 0.70, 0.0]
-        q = wp.quat_from_axis_angle(wp.vec3(1.0, 0.0, 0.0), -math.pi * 0.5)
-        builder.joint_q[coord_start : coord_start + 3] = p
-        builder.joint_q[coord_start + 3 : coord_start + 7] = q
-
-        # joints
-        builder.joint_q[coord_start + 7 : coord_start + coord_count] = [0.0, 1.0, 0.0, -1.0, 0.0, -1.0, 0.0, 1.0]
-        builder.joint_qd[dof_start + 6 : dof_start + dof_count] = [1.0, 1.0, 1.0, -1.0, 1.0, -1.0, 1.0, 1.0]
-
-    return builder
-
-
-def build_complex_joint_mechanism(chain_length):
-    builder = wp.sim.ModelBuilder()
-    com0 = wp.vec3(1.0, 2.0, 3.0)
-    com1 = wp.vec3(4.0, 5.0, 6.0)
-    com2 = wp.vec3(7.0, 8.0, 9.0)
-    ax0 = wp.normalize(wp.vec3(-1.0, 2.0, 3.0))
-    ax1 = wp.normalize(wp.vec3(4.0, -1.0, 2.0))
-    ax2 = wp.normalize(wp.vec3(-3.0, 4.0, -1.0))
-    # declare some transforms with nonzero translation and orientation
-    tf0 = wp.transform(wp.vec3(1.0, 2.0, 3.0), wp.quat_from_axis_angle(wp.vec3(1.0, 0.0, 0.0), math.pi * 0.25))
-    tf1 = wp.transform(wp.vec3(4.0, 5.0, 6.0), wp.quat_from_axis_angle(wp.vec3(0.0, 1.0, 0.0), math.pi * 0.5))
-    tf2 = wp.transform(wp.vec3(7.0, 8.0, 9.0), wp.quat_from_axis_angle(wp.vec3(0.0, 0.0, 1.0), math.pi * 0.75))
-
-    parent = -1
-    for _i in range(chain_length):
-        b0 = builder.add_body(com=com0)
-        builder.add_joint_fixed(parent=parent, child=b0, parent_xform=tf1, child_xform=tf0)
-        assert builder.articulation_count == 1
-
-        b1 = builder.add_body(com=com1)
-        builder.add_joint_revolute(parent=b0, child=b1, parent_xform=tf1, child_xform=tf2, axis=ax1)
-        builder.joint_q[-1] = 0.3
-        builder.joint_qd[-1] = 1.0
-
-        b2 = builder.add_body(com=com2)
-        builder.add_joint_universal(parent=b1, child=b2, parent_xform=tf2, child_xform=tf0, axis_0=ax0, axis_1=ax1)
-        builder.joint_q[-2:] = [0.3, 0.5]
-        builder.joint_qd[-2:] = [1.0, -1.0]
-
-        b3 = builder.add_body(com=com0)
-        builder.add_joint_ball(parent=b2, child=b3, parent_xform=tf0, child_xform=tf1)
-        builder.joint_q[-4:] = list(wp.quat_from_axis_angle(ax0, 0.7))
-        builder.joint_qd[-3:] = [1.0, -0.6, 1.5]
-
-        b4 = builder.add_body(com=com1)
-        builder.add_joint_compound(
-            parent=b3,
-            child=b4,
-            parent_xform=tf2,
-            child_xform=tf1,
-            axis_0=(0, 0, 1),
-            axis_1=(1, 0, 0),
-            axis_2=(0, 1, 0),
-        )
-        builder.joint_q[-3:] = [0.3, 0.5, 0.27]
-        builder.joint_qd[-3:] = [1.23, -1.0, 0.5]
-
-        b5 = builder.add_body(com=com2)
-        builder.add_joint_prismatic(
-            parent=b4,
-            child=b5,
-            parent_xform=tf2,
-            child_xform=tf0,
-            axis=ax0,
-        )
-        builder.joint_q[-1] = 0.92
-        builder.joint_qd[-1] = -0.63
-
-        b6 = builder.add_body(com=com0)
-        builder.add_joint_d6(
-            parent=b5,
-            child=b6,
-            parent_xform=tf0,
-            child_xform=tf2,
-            linear_axes=[ax0, ax1, wp.cross(ax0, ax1)],
-            angular_axes=[ax1, ax2, wp.cross(ax1, ax2)],
-        )
-        builder.joint_q[-6:] = [0.3, 0.5, 0.7, 0.9, 1.1, 1.3]
-        builder.joint_qd[-6:] = [1.0, -1.0, 0.5, 0.8, -0.3, 0.1]
-
-        b7 = builder.add_body(com=com1)
-        builder.add_joint_free(
-            parent=b6,
-            child=b7,
-            parent_xform=tf1,
-            child_xform=tf2,
-        )
-        builder.joint_q[-7:] = [0.5, -0.9, 1.4, *wp.quat_rpy(0.3, -0.5, 0.7)]
-        builder.joint_qd[-6:] = [1.0, -1.0, 0.5, 0.8, -0.3, 0.1]
-
-        b8 = builder.add_body(com=com2)
-        builder.add_joint_distance(
-            parent=b7,
-            child=b8,
-            parent_xform=tf1,
-            child_xform=tf2,
-        )
-        builder.joint_q[-7:] = [-0.3, -0.7, 0.2, *wp.quat_rpy(0.1, 0.1, 0.4)]
-        builder.joint_qd[-6:] = [-0.34, 0.5, -0.6, -0.4, 0.2, 0.1]
-
-        # D6 joint that behaves like a fixed joint
-        b9 = builder.add_body(com=com0)
-        builder.add_joint_d6(
-            parent=b8,
-            child=b9,
-            parent_xform=tf0,
-            child_xform=tf2,
-            linear_axes=[],
-            angular_axes=[],
-        )
-
-        b10 = builder.add_body(com=com0)
-        builder.add_joint_d6(
-            parent=b9,
-            child=b10,
-            parent_xform=tf1,
-            child_xform=tf2,
-            linear_axes=[ax1],
-            angular_axes=[ax2, ax0],
-        )
-        builder.joint_q[-3:] = [0.3, 0.5, 0.7]
-        builder.joint_qd[-3:] = [1.0, -1.0, 0.5]
-
-        b11 = builder.add_body(com=com1)
-        builder.add_joint_d6(
-            parent=b10,
-            child=b11,
-            parent_xform=tf1,
-            child_xform=tf2,
-            linear_axes=[ax1, ax0, wp.cross(ax1, ax0)],
-            angular_axes=[],
-        )
-        builder.joint_q[-3:] = [0.3, 0.5, 0.7]
-        builder.joint_qd[-3:] = [1.0, -1.0, 0.5]
-
-        b12 = builder.add_body(com=com2)
-        builder.add_joint_d6(
-            parent=b11,
-            child=b12,
-            parent_xform=tf1,
-            child_xform=tf2,
-            linear_axes=[],
-            angular_axes=[ax1, ax2, wp.cross(ax1, ax2)],
-        )
-        builder.joint_q[-3:] = [0.3, 0.5, 0.7]
-        builder.joint_qd[-3:] = [1.0, -1.0, 0.5]
-
-        parent = b12
-
-    return builder
-
-
-def check_fk_ik(builder, device):
-    model = builder.finalize(device)
-    state = model.state()
-
-    q_fk = model.joint_q.numpy()
-    qd_fk = model.joint_qd.numpy()
-
-    wp.sim.eval_fk(model, model.joint_q, model.joint_qd, None, state)
-
-    q_ik = wp.zeros_like(model.joint_q)
-    qd_ik = wp.zeros_like(model.joint_qd)
-
-    wp.sim.eval_ik(model, state, q_ik, qd_ik)
-
-    # adjust numpy print settings
-    # np.set_printoptions(precision=4, floatmode="fixed", suppress=True)
-    # print("q:")
-    # print(np.array(q_fk))
-    # print(q_ik.numpy())
-
-    # print("qd:")
-    # print(np.array(qd_fk))
-    # print(qd_ik.numpy())
-
-    assert_np_equal(q_ik.numpy(), q_fk, tol=1e-4)
-    assert_np_equal(qd_ik.numpy(), qd_fk, tol=1e-4)
-
-
-def test_fk_ik_ant(test, device):
-    builder = build_ant(3)
-    check_fk_ik(builder, device)
-
-
-def test_fk_ik_complex_joint_mechanism(test, device):
-    builder = build_complex_joint_mechanism(2)
-    check_fk_ik(builder, device)
-
-
-devices = get_test_devices()
-
-
-class TestKinematics(unittest.TestCase):
-    pass
-
-
-add_function_test(TestKinematics, "test_fk_ik_ant", test_fk_ik_ant, devices=devices)
-add_function_test(
-    TestKinematics, "test_fk_ik_complex_joint_mechanism", test_fk_ik_complex_joint_mechanism, devices=devices
-)
-
-
-if __name__ == "__main__":
-    wp.clear_kernel_cache()
-    unittest.main(verbosity=2, failfast=False)