fix wp.index error and minor bugfixes

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

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
 

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

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)