From ec99bfba75ab5a2bb26423bc29f2667b3babd849 Mon Sep 17 00:00:00 2001 From: Balakumar Sundaralingam Date: Wed, 24 Jan 2024 23:15:30 -0800 Subject: [PATCH] fix wp.index error and minor bugfixes --- CHANGELOG.md | 7 +++++-- src/curobo/cuda_robot_model/cuda_robot_model.py | 2 +- src/curobo/geom/transform.py | 16 ++++++++-------- src/curobo/rollout/rollout_base.py | 4 ++-- src/curobo/types/state.py | 2 +- 5 files changed, 17 insertions(+), 14 deletions(-) diff --git a/CHANGELOG.md b/CHANGELOG.md index 587ed07..e21aaa7 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -9,6 +9,11 @@ without an express license agreement from NVIDIA CORPORATION or its affiliates is strictly prohibited. --> # Changelog +## Latest Commit +### BugFixes +- refactored wp.index() instances to `[]` to avoid errors in using with warp-lang>=0.11. +- Fix bug in gaussian transformation to ensure values are not -1 or +1. +- Fixed bug in transforming link visual mesh offset when reading from urdf. ## Version 0.6.2 ### New Features @@ -30,7 +35,6 @@ Run `benchmark/ik_benchmark.py` to get the latest results. - Added `external_asset_path` to robot configuration to help in loading urdf and meshes from an external directory. - ### BugFixes & Misc. - Update nvblox wrappers to work with v0.0.5 without segfaults. Significantly improves stability. - Remove mimic joints in franka panda to maintain compatibility with Isaac Sim 2023.1.0 and 2022.2.1 @@ -56,7 +60,6 @@ planning time, 20ms slower on 4090 and 40ms on ORIN MAXN. We will address this s release. One way to avoid this regression is to set `finetune_dt_scale=1.05` in `MotionGenConfig.load_from_robot_config()`. - ## Version 0.6.1 - Added changes to `examples/isaac_sim` to support Isaac Sim 2023.1.0 diff --git a/src/curobo/cuda_robot_model/cuda_robot_model.py b/src/curobo/cuda_robot_model/cuda_robot_model.py index 2fcaeaa..78f4370 100644 --- a/src/curobo/cuda_robot_model/cuda_robot_model.py +++ b/src/curobo/cuda_robot_model/cuda_robot_model.py @@ -264,7 +264,7 @@ class CudaRobotModel(CudaRobotModelConfig): pose = self.get_link_poses(q, self.kinematics_config.mesh_link_names) for li, l in enumerate(self.kinematics_config.mesh_link_names): m_list[li].pose = ( - Pose.from_list(m_list[li].pose).multiply(pose.get_index(0, li)).tolist() + pose.get_index(0, li).multiply(Pose.from_list(m_list[li].pose)).tolist() ) return m_list diff --git a/src/curobo/geom/transform.py b/src/curobo/geom/transform.py index 606358c..cf802cc 100644 --- a/src/curobo/geom/transform.py +++ b/src/curobo/geom/transform.py @@ -353,10 +353,10 @@ def compute_pose_inverse( out_q = wp.transform_get_rotation(t_3) out_v = wp.vec4() - out_v[0] = wp.index(out_q, 3) - out_v[1] = wp.index(out_q, 0) - out_v[2] = wp.index(out_q, 1) - out_v[3] = wp.index(out_q, 2) + out_v[0] = out_q[3] + out_v[1] = out_q[0] + out_v[2] = out_q[1] + out_v[3] = out_q[2] out_position[b_idx] = wp.transform_get_translation(t_3) out_quat[b_idx] = out_v @@ -398,10 +398,10 @@ def compute_matrix_to_quat( out_q = wp.quat_from_matrix(in_m) out_v = wp.vec4() - out_v[0] = wp.index(out_q, 3) - out_v[1] = wp.index(out_q, 0) - out_v[2] = wp.index(out_q, 1) - out_v[3] = wp.index(out_q, 2) + out_v[0] = out_q[3] + out_v[1] = out_q[0] + out_v[2] = out_q[1] + out_v[3] = out_q[2] # write pt: out_quat[b_idx] = out_v diff --git a/src/curobo/rollout/rollout_base.py b/src/curobo/rollout/rollout_base.py index c6904d9..a5edb46 100644 --- a/src/curobo/rollout/rollout_base.py +++ b/src/curobo/rollout/rollout_base.py @@ -23,7 +23,7 @@ import torch # CuRobo from curobo.types.base import TensorDeviceType from curobo.types.math import Pose -from curobo.types.robot import CSpaceConfig, State +from curobo.types.robot import CSpaceConfig, State, JointState from curobo.types.tensor import ( T_BDOF, T_DOF, @@ -456,7 +456,7 @@ class RolloutBase: return out_metrics def get_metrics_cuda_graph(self, state: State): - return get_metrics(state) + return self.get_metrics(state) def rollout_fn(self, act): pass diff --git a/src/curobo/types/state.py b/src/curobo/types/state.py index 5ba1db6..aa051e9 100644 --- a/src/curobo/types/state.py +++ b/src/curobo/types/state.py @@ -182,7 +182,7 @@ class JointState(State): if velocity is None: velocity = self.position * 0.0 if acceleration is None: - acceleration = self.positoin * 0.0 + acceleration = self.position * 0.0 if jerk is None: jerk = self.position * 0.0 state_tensor = torch.cat((self.position, velocity, acceleration, jerk), dim=-1)