Add output path and recorder configuration to benchmark.yaml;

benchmark.yaml

@@ -3,6 +3,7 @@ general:
   root_paths:
     asset: /home/ubuntu/projects/gen_data/data
     checkpoints: /home/ubuntu/data/models
+    output: /home/ubuntu/output
 
 simulation:
   launch_config:
@@ -205,6 +206,13 @@ extension:
         sensor_names: [Hand_Camera, Left_Camera, Right_Camera]
         prompt: pick the cola bottle and place it on the book
 
+    recorder:
+      enable: false # set to true to record the data
+      stereotype: record
+      data_collector_name: benchmark_data_collect
+      record_fps: 30
+      backend_root_path: output://benchmark_record
+
 policy_server:
   ckpt_path: checkpoints://0309_qwenpi_droid_cola_post/final_model/pytorch_model.pt
   ckpt_source: local

starvla_policy.py

@@ -2,9 +2,7 @@ import pickle
 
 from joysim.annotations.config_class import configclass, field
 from joysim.annotations.stereotype import stereotype
-from joysim.app import JoySim
 from joysim.controllers.motion_plan_controller import MotionPlanController
-from joysim.core.scene_manager import SceneManager
 from joysim.extensions.benchmark.action import RobotAction
 from joysim.extensions.benchmark.benchmark import (
     BenchmarkAction,
@@ -21,7 +19,7 @@ import requests
 @stereotype.register_config("starvla")
 class StarvlaPolicyConfig(PolicyConfig):
 
-    robot_name: str = field(default="my_robot", required=True, comment="The name of the robot")
+    robot_name: str = field(default="None", required=True, comment="The name of the robot")
     sensor_names: list[str] = field(
         default=["Hand_Camera", "Left_Camera", "Right_Camera"],
         required=True,
@@ -75,7 +73,7 @@ class StarvlaPolicy(Policy):
         robot_obs = benchmark_observation.get_robot_observations(self.robot_name)["robot_data"]
         ee_pose_base = robot_obs["ee_pose_base"]
         ee_position, ee_euler_xyz = ee_pose_base["position"],ee_pose_base["euler_xyz"]
-        gripper = 1.0 if robot_obs["gripper_state"]["opened"] else 0.0
+        gripper = 0.0 if robot_obs["gripper_state"]["opened"] else 1.0
         state = np.concatenate([ee_position,ee_euler_xyz,np.array([gripper])])
         self.current_ee_position_state = np.array(ee_position).astype(np.float64)
         self.current_ee_euler_xyz_state = np.array(ee_euler_xyz).astype(np.float64)
@@ -102,17 +100,16 @@ class StarvlaPolicy(Policy):
     def postprocess_action(self, action: dict) -> BenchmarkAction:
         benchmark_action = BenchmarkAction()
 
-        # get base frame end-effector pose # TODO: Make sure add or multiply the current state
+        # get base frame end-effector pose
-        ee_position = action["ee_delta_position_chunks"][0] + self.current_ee_position_state
+        delta_ee_pose = Pose(position=action["ee_delta_position_chunks"][0], euler_xyz=action["ee_delta_euler_xyz_chunks"][0])
-        ee_euler_xyz = action["ee_delta_euler_xyz_chunks"][0] + self.current_ee_euler_xyz_state
+        curr_state_ee_pose = Pose(position=self.current_ee_position_state, euler_xyz=self.current_ee_euler_xyz_state)
-
+        curr_action_ee_pose = curr_state_ee_pose * delta_ee_pose # action2base = state2base * action2state
-        ee_pose = Pose(position=ee_position, euler_xyz=ee_euler_xyz)
         ik_result = MotionPlanController.solve_ik(
             robot_name=self.robot_name,
-            base_frame_ee_pose=ee_pose,
+            base_frame_ee_pose=curr_action_ee_pose,
         ).unwrap()
         if not ik_result["success"]:
-            Log.error(f"IK failed: {ik_result['status']}. Ignore this action.")
+            Log.error(f"IK failed. Ignore this action.")
             return benchmark_action
         
         joint_names = ik_result["result"]["plannable_joint_names"]
@@ -127,8 +124,3 @@ class StarvlaPolicy(Policy):
         )
 
         return benchmark_action
-
-
-if __name__ == "__main__":
-    js = JoySim("/home/ubuntu/projects/benchmark/benchmark.yaml")
-    js.start()