fix ik (Genesis-Embodied-AI#1577)

Author: YilingQiao

examples/tutorials/IK_motion_planning_grasp.py

@@ -64,15 +64,15 @@
     num_waypoints=200,  # 2s duration
 )
 # draw the planned path
-# path_debug = scene.draw_debug_path(path, franka)
+path_debug = scene.draw_debug_path(path, franka)
 
 # execute the planned path
 for waypoint in path:
     franka.control_dofs_position(waypoint)
     scene.step()
 
 # remove the drawn path
-# scene.clear_debug_object(path_debug)
+scene.clear_debug_object(path_debug)
 
 # allow robot to reach the last waypoint
 for i in range(100):

genesis/engine/entities/rigid_entity/rigid_entity.py

@@ -1326,7 +1326,7 @@ def _kernel_forward_kinematics(
             # set new qpos
             self._solver.qpos[qs_idx[i_q_], envs_idx[i_b_]] = qpos[i_b_, i_q_]
             # run FK
-            self._solver.func_forward_kinematics_entity(
+            gs.engine.solvers.rigid.rigid_solver_decomp.func_forward_kinematics_entity(
                 self._idx_in_solver,
                 envs_idx[i_b_],
                 self._solver.links_state,
@@ -1343,16 +1343,16 @@ def _kernel_forward_kinematics(
         ti.loop_config(serialize=self._solver._para_level < gs.PARA_LEVEL.PARTIAL)
         for i_l_, i_b_ in ti.ndrange(links_idx.shape[0], envs_idx.shape[0]):
             for i in ti.static(range(3)):
-                links_pos[i_b_, i_l_, i] = self._solver.links_state.pos[links_idx[i_l_], envs_idx[i_b_]]
+                links_pos[i_b_, i_l_, i] = self._solver.links_state.pos[links_idx[i_l_], envs_idx[i_b_]][i]
             for i in ti.static(range(4)):
-                links_quat[i_b_, i_l_, i] = self._solver.links_state.quat[links_idx[i_l_], envs_idx[i_b_]]
+                links_quat[i_b_, i_l_, i] = self._solver.links_state.quat[links_idx[i_l_], envs_idx[i_b_]][i]
 
         ti.loop_config(serialize=self._solver._para_level < gs.PARA_LEVEL.ALL)
         for i_q_, i_b_ in ti.ndrange(qs_idx.shape[0], envs_idx.shape[0]):
             # restore original qpos
             self._solver.qpos[qs_idx[i_q_], envs_idx[i_b_]] = self._IK_qpos_orig[qs_idx[i_q_], envs_idx[i_b_]]
             # run FK
-            self._solver.func_forward_kinematics_entity(
+            gs.engine.solvers.rigid.rigid_solver_decomp.func_forward_kinematics_entity(
                 self._idx_in_solver,
                 envs_idx[i_b_],
                 self._solver.links_state,