warp-lang 1.9.1__py3-none-manylinux_2_34_aarch64.whl → 1.10.0rc2__py3-none-manylinux_2_34_aarch64.whl

warp/examples/sim/example_jacobian_ik.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.
-
-###########################################################################
-# Example Jacobian
-#
-# Demonstrates how to compute the Jacobian of a multi-valued function.
-# Here, we use the simulation of a cartpole to differentiate
-# through the kinematics function. We instantiate multiple copies of the
-# cartpole and compute the Jacobian of the state of each cartpole in parallel
-# in order to perform inverse kinematics via Jacobian transpose.
-#
-###########################################################################
-
-import math
-import os
-
-import numpy as np
-
-import warp as wp
-import warp.examples
-import warp.sim
-import warp.sim.render
-
-
-@wp.kernel
-def compute_endeffector_position(
-    body_q: wp.array(dtype=wp.transform),
-    num_links: int,
-    ee_link_index: int,
-    ee_link_offset: wp.vec3,
-    ee_pos: wp.array(dtype=wp.vec3),
-):
-    tid = wp.tid()
-    ee_pos[tid] = wp.transform_point(body_q[tid * num_links + ee_link_index], ee_link_offset)
-
-
-class Example:
-    def __init__(self, stage_path="example_jacobian_ik.usd", num_envs=10):
-        rng = np.random.default_rng(42)
-
-        self.num_envs = num_envs
-
-        fps = 60
-        self.frame_dt = 1.0 / fps
-
-        self.render_time = 0.0
-
-        # step size to use for the IK updates
-        self.step_size = 0.1
-
-        articulation_builder = wp.sim.ModelBuilder()
-
-        wp.sim.parse_urdf(
-            os.path.join(warp.examples.get_asset_directory(), "cartpole.urdf"),
-            articulation_builder,
-            xform=wp.transform_identity(),
-            floating=False,
-        )
-
-        builder = wp.sim.ModelBuilder()
-
-        self.num_links = len(articulation_builder.joint_type)
-        # use the last link as the end-effector
-        self.ee_link_index = self.num_links - 1
-        self.ee_link_offset = wp.vec3(0.0, 0.0, 1.0)
-
-        self.dof = len(articulation_builder.joint_q)
-
-        self.target_origin = []
-        for i in range(self.num_envs):
-            builder.add_builder(
-                articulation_builder,
-                xform=wp.transform(
-                    wp.vec3(i * 2.0, 4.0, 0.0), wp.quat_from_axis_angle(wp.vec3(1.0, 0.0, 0.0), -math.pi * 0.5)
-                ),
-            )
-            self.target_origin.append((i * 2.0, 4.0, 0.0))
-            # joint initial positions
-            builder.joint_q[-3:] = rng.uniform(-0.5, 0.5, size=3)
-        self.target_origin = np.array(self.target_origin)
-
-        # finalize model
-        self.model = builder.finalize()
-        self.model.ground = True
-
-        self.model.joint_q.requires_grad = True
-        self.model.body_q.requires_grad = True
-
-        self.model.joint_attach_ke = 1600.0
-        self.model.joint_attach_kd = 20.0
-
-        self.integrator = wp.sim.SemiImplicitIntegrator()
-
-        if stage_path:
-            self.renderer = wp.sim.render.SimRenderer(self.model, stage_path)
-        else:
-            self.renderer = None
-
-        self.ee_pos = wp.zeros(self.num_envs, dtype=wp.vec3, requires_grad=True)
-
-        self.state = self.model.state(requires_grad=True)
-
-        self.targets = self.target_origin.copy()
-
-        self.profiler = {}
-
-    def compute_ee_position(self):
-        # computes the end-effector position from the current joint angles
-        wp.sim.eval_fk(self.model, self.model.joint_q, self.model.joint_qd, None, self.state)
-        wp.launch(
-            compute_endeffector_position,
-            dim=self.num_envs,
-            inputs=[self.state.body_q, self.num_links, self.ee_link_index, self.ee_link_offset],
-            outputs=[self.ee_pos],
-        )
-        return self.ee_pos
-
-    def compute_jacobian(self):
-        # our function has 3 outputs (EE position), so we need a 3xN jacobian per environment
-        jacobians = np.empty((self.num_envs, 3, self.dof), dtype=np.float32)
-        tape = wp.Tape()
-        with tape:
-            self.compute_ee_position()
-        for output_index in range(3):
-            # select which row of the Jacobian we want to compute
-            select_index = np.zeros(3)
-            select_index[output_index] = 1.0
-            e = wp.array(np.tile(select_index, self.num_envs), dtype=wp.vec3)
-            tape.backward(grads={self.ee_pos: e})
-            q_grad_i = tape.gradients[self.model.joint_q]
-            jacobians[:, output_index, :] = q_grad_i.numpy().reshape(self.num_envs, self.dof)
-            tape.zero()
-        return jacobians
-
-    def compute_fd_jacobian(self, eps=1e-4):
-        jacobians = np.zeros((self.num_envs, 3, self.dof), dtype=np.float32)
-        q0 = self.model.joint_q.numpy().copy()
-        for e in range(self.num_envs):
-            for i in range(self.dof):
-                q = q0.copy()
-                q[e * self.dof + i] += eps
-                self.model.joint_q.assign(q)
-                self.compute_ee_position()
-                f_plus = self.ee_pos.numpy()[e].copy()
-                q[e * self.dof + i] -= 2 * eps
-                self.model.joint_q.assign(q)
-                self.compute_ee_position()
-                f_minus = self.ee_pos.numpy()[e].copy()
-                jacobians[e, :, i] = (f_plus - f_minus) / (2 * eps)
-        return jacobians
-
-    def step(self):
-        with wp.ScopedTimer("jacobian", print=False, active=True, dict=self.profiler):
-            # compute jacobian
-            jacobians = self.compute_jacobian()
-
-        # compute error
-        self.ee_pos_np = self.compute_ee_position().numpy()
-        error = self.targets - self.ee_pos_np
-        self.error = error.reshape(self.num_envs, 3, 1)
-
-        # compute Jacobian transpose update
-        delta_q = np.matmul(jacobians.transpose(0, 2, 1), self.error)
-
-        self.model.joint_q = wp.array(
-            self.model.joint_q.numpy() + self.step_size * delta_q.flatten(),
-            dtype=wp.float32,
-            requires_grad=True,
-        )
-
-    def render(self):
-        if self.renderer is None:
-            return
-
-        self.renderer.begin_frame(self.render_time)
-        self.renderer.render(self.state)
-        self.renderer.render_points("targets", self.targets, radius=0.05)
-        self.renderer.render_points("ee_pos", self.ee_pos_np, radius=0.05)
-        self.renderer.end_frame()
-        self.render_time += self.frame_dt
-
-
-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_jacobian_ik.usd",
-        help="Path to the output USD file.",
-    )
-    parser.add_argument("--train_iters", type=int, default=50, help="Total number of training iterations.")
-    parser.add_argument("--num_envs", type=int, default=10, help="Total number of simulated environments.")
-    parser.add_argument(
-        "--num_rollouts",
-        type=int,
-        default=5,
-        help="Total number of rollouts. In each rollout, a new set of target points is resampled for all environments.",
-    )
-
-    args = parser.parse_known_args()[0]
-
-    rng = np.random.default_rng(42)
-
-    with wp.ScopedDevice(args.device):
-        example = Example(stage_path=args.stage_path, num_envs=args.num_envs)
-
-        print("autodiff:")
-        print(example.compute_jacobian())
-        print("finite diff:")
-        print(example.compute_fd_jacobian())
-
-        for _ in range(args.num_rollouts):
-            # select new random target points for all envs
-            example.targets = example.target_origin.copy()
-            example.targets[:, 1:] += rng.uniform(-0.5, 0.5, size=(example.num_envs, 2))
-
-            for iter in range(args.train_iters):
-                example.step()
-                example.render()
-                print("iter:", iter, "error:", example.error.mean())
-
-        if example.renderer:
-            example.renderer.save()
-
-        avg_time = np.array(example.profiler["jacobian"]).mean()
-        avg_steps_second = 1000.0 * float(example.num_envs) / avg_time
-
-        print(f"envs: {example.num_envs} steps/second {avg_steps_second} avg_time {avg_time}")

warp/examples/sim/example_particle_chain.py

@@ -1,124 +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.
-
-###########################################################################
-# Example Sim Particle Chain
-#
-# Shows how to set up a simple chain of particles connected by springs
-# using wp.sim.ModelBuilder().
-#
-###########################################################################
-
-import math
-
-import warp as wp
-import warp.sim
-import warp.sim.render
-
-
-class Example:
-    def __init__(self, stage_path="example_particle_chain.usd"):
-        self.sim_width = 64
-        self.sim_height = 32
-
-        fps = 60
-        self.frame_dt = 1.0 / fps
-        self.sim_substeps = 10
-        self.sim_dt = self.frame_dt / self.sim_substeps
-        self.sim_time = 0.0
-
-        builder = wp.sim.ModelBuilder()
-
-        # anchor
-        builder.add_particle(wp.vec3(0.0, 1.0, 0.0), wp.vec3(0.0, 0.0, 0.0), 0.0)
-
-        # chain
-        for i in range(1, 10):
-            radius = math.sqrt(i) * 0.2
-            mass = math.pi * radius * radius * radius
-            builder.add_particle(wp.vec3(i, 1.0, 0.0), wp.vec3(0.0, 0.0, 0.0), mass, radius=radius)
-            builder.add_spring(i - 1, i, 1.0e6, 0.0, 0)
-
-        self.model = builder.finalize()
-        self.model.ground = False
-
-        self.integrator = wp.sim.XPBDIntegrator()
-
-        self.state_0 = self.model.state()
-        self.state_1 = self.model.state()
-
-        if stage_path:
-            self.renderer = wp.sim.render.SimRenderer(self.model, stage_path, scaling=15.0)
-        else:
-            self.renderer = None
-
-        # simulate() allocates memory via a clone, so we can't use graph capture if the device does not support mempools
-        self.use_cuda_graph = wp.get_device().is_cuda and wp.is_mempool_enabled(wp.get_device())
-        if self.use_cuda_graph:
-            with wp.ScopedCapture() as capture:
-                self.simulate()
-            self.graph = capture.graph
-
-    def simulate(self):
-        for _ in range(self.sim_substeps):
-            self.state_0.clear_forces()
-            self.state_1.clear_forces()
-            self.integrator.simulate(self.model, self.state_0, self.state_1, self.sim_dt)
-
-            # swap states
-            (self.state_0, self.state_1) = (self.state_1, self.state_0)
-
-    def step(self):
-        with wp.ScopedTimer("step"):
-            if self.use_cuda_graph:
-                wp.capture_launch(self.graph)
-            else:
-                self.simulate()
-        self.sim_time += self.frame_dt
-
-    def render(self):
-        if self.renderer is None:
-            return
-
-        with wp.ScopedTimer("render"):
-            self.renderer.begin_frame(self.sim_time)
-            self.renderer.render(self.state_0)
-            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_particle_chain.usd",
-        help="Path to the output USD file.",
-    )
-    parser.add_argument("--num_frames", type=int, default=300, help="Total number of frames.")
-
-    args = parser.parse_known_args()[0]
-
-    with wp.ScopedDevice(args.device):
-        example = Example(stage_path=args.stage_path)
-
-        for _ in range(args.num_frames):
-            example.step()
-            example.render()
-
-        if example.renderer:
-            example.renderer.save()

warp/examples/sim/example_quadruped.py

@@ -1,203 +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.
-
-###########################################################################
-# Example Sim Quadruped
-#
-# Shows how to set up a simulation of a rigid-body quadruped articulation
-# from a URDF using the wp.sim.ModelBuilder().
-# Note this example does not include a trained policy.
-#
-###########################################################################
-
-import math
-import os
-
-import numpy as np
-
-import warp as wp
-import warp.examples
-import warp.sim
-import warp.sim.render
-
-
-# Taken from env/environment.py
-def compute_env_offsets(num_envs, env_offset=(5.0, 0.0, 5.0), up_axis="Y"):
-    # compute positional offsets per environment
-    env_offset = np.array(env_offset)
-    nonzeros = np.nonzero(env_offset)[0]
-    num_dim = nonzeros.shape[0]
-    if num_dim > 0:
-        side_length = int(np.ceil(num_envs ** (1.0 / num_dim)))
-        env_offsets = []
-    else:
-        env_offsets = np.zeros((num_envs, 3))
-    if num_dim == 1:
-        for i in range(num_envs):
-            env_offsets.append(i * env_offset)
-    elif num_dim == 2:
-        for i in range(num_envs):
-            d0 = i // side_length
-            d1 = i % side_length
-            offset = np.zeros(3)
-            offset[nonzeros[0]] = d0 * env_offset[nonzeros[0]]
-            offset[nonzeros[1]] = d1 * env_offset[nonzeros[1]]
-            env_offsets.append(offset)
-    elif num_dim == 3:
-        for i in range(num_envs):
-            d0 = i // (side_length * side_length)
-            d1 = (i // side_length) % side_length
-            d2 = i % side_length
-            offset = np.zeros(3)
-            offset[0] = d0 * env_offset[0]
-            offset[1] = d1 * env_offset[1]
-            offset[2] = d2 * env_offset[2]
-            env_offsets.append(offset)
-    env_offsets = np.array(env_offsets)
-    min_offsets = np.min(env_offsets, axis=0)
-    correction = min_offsets + (np.max(env_offsets, axis=0) - min_offsets) / 2.0
-    if isinstance(up_axis, str):
-        up_axis = "XYZ".index(up_axis.upper())
-    correction[up_axis] = 0.0  # ensure the envs are not shifted below the ground plane
-    env_offsets -= correction
-    return env_offsets
-
-
-class Example:
-    def __init__(self, stage_path="example_quadruped.usd", num_envs=8):
-        articulation_builder = wp.sim.ModelBuilder()
-        wp.sim.parse_urdf(
-            os.path.join(warp.examples.get_asset_directory(), "quadruped.urdf"),
-            articulation_builder,
-            xform=wp.transform([0.0, 0.7, 0.0], wp.quat_from_axis_angle(wp.vec3(1.0, 0.0, 0.0), -math.pi * 0.5)),
-            floating=True,
-            density=1000,
-            armature=0.01,
-            stiffness=200,
-            damping=1,
-            contact_ke=1.0e4,
-            contact_kd=1.0e2,
-            contact_kf=1.0e2,
-            contact_mu=1.0,
-            limit_ke=1.0e4,
-            limit_kd=1.0e1,
-        )
-
-        builder = wp.sim.ModelBuilder()
-
-        self.sim_time = 0.0
-        fps = 100
-        self.frame_dt = 1.0 / fps
-
-        self.sim_substeps = 10
-        self.sim_dt = self.frame_dt / self.sim_substeps
-
-        self.num_envs = num_envs
-
-        offsets = compute_env_offsets(self.num_envs)
-        for i in range(self.num_envs):
-            builder.add_builder(articulation_builder, xform=wp.transform(offsets[i], wp.quat_identity()))
-
-            builder.joint_q[-12:] = [0.2, 0.4, -0.6, -0.2, -0.4, 0.6, -0.2, 0.4, -0.6, 0.2, -0.4, 0.6]
-
-            builder.joint_axis_mode = [wp.sim.JOINT_MODE_TARGET_POSITION] * len(builder.joint_axis_mode)
-            builder.joint_act[-12:] = [0.2, 0.4, -0.6, -0.2, -0.4, 0.6, -0.2, 0.4, -0.6, 0.2, -0.4, 0.6]
-
-        np.set_printoptions(suppress=True)
-        # finalize model
-        self.model = builder.finalize()
-        self.model.ground = True
-        # self.model.gravity = 0.0
-
-        self.model.joint_attach_ke = 16000.0
-        self.model.joint_attach_kd = 200.0
-        self.use_tile_gemm = False
-        self.fuse_cholesky = False
-
-        # self.integrator = wp.sim.XPBDIntegrator()
-        # self.integrator = wp.sim.SemiImplicitIntegrator()
-        self.integrator = wp.sim.FeatherstoneIntegrator(
-            self.model, use_tile_gemm=self.use_tile_gemm, fuse_cholesky=self.fuse_cholesky
-        )
-
-        if stage_path:
-            self.renderer = wp.sim.render.SimRenderer(self.model, stage_path)
-        else:
-            self.renderer = None
-
-        self.state_0 = self.model.state()
-        self.state_1 = self.model.state()
-
-        wp.sim.eval_fk(self.model, self.model.joint_q, self.model.joint_qd, None, self.state_0)
-
-        # simulate() allocates memory via a clone, so we can't use graph capture if the device does not support mempools
-        self.use_cuda_graph = wp.get_device().is_cuda and wp.is_mempool_enabled(wp.get_device())
-        if self.use_cuda_graph:
-            with wp.ScopedCapture() as capture:
-                self.simulate()
-            self.graph = capture.graph
-        else:
-            self.graph = None
-
-    def simulate(self):
-        for _ in range(self.sim_substeps):
-            self.state_0.clear_forces()
-            wp.sim.collide(self.model, self.state_0)
-            self.integrator.simulate(self.model, self.state_0, self.state_1, self.sim_dt)
-            self.state_0, self.state_1 = self.state_1, self.state_0
-
-    def step(self):
-        with wp.ScopedTimer("step"):
-            if self.use_cuda_graph:
-                wp.capture_launch(self.graph)
-            else:
-                self.simulate()
-        self.sim_time += self.frame_dt
-
-    def render(self):
-        if self.renderer is None:
-            return
-
-        with wp.ScopedTimer("render"):
-            self.renderer.begin_frame(self.sim_time)
-            self.renderer.render(self.state_0)
-            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_quadruped.usd",
-        help="Path to the output USD file.",
-    )
-    parser.add_argument("--num_frames", type=int, default=300, help="Total number of frames.")
-    parser.add_argument("--num_envs", type=int, default=8, help="Total number of simulated environments.")
-
-    args = parser.parse_known_args()[0]
-
-    with wp.ScopedDevice(args.device):
-        example = Example(stage_path=args.stage_path, num_envs=args.num_envs)
-
-        for _ in range(args.num_frames):
-            example.step()
-            example.render()
-
-        if example.renderer:
-            example.renderer.save()