Add create_action_vector() for 'basic' controller; converts body part…

… key/vector into full vector

robosuite/controllers/composite/composite_controller.py

@@ -43,6 +43,7 @@ def __init__(self, sim: MjSim, robot_model: RobotModel, grippers: Dict[str, Grip
         self.part_controllers = OrderedDict()
 
         self._action_split_indexes = OrderedDict()
+        self._action_split_joint_names = OrderedDict()
 
         self.part_controller_config = None
 
@@ -65,6 +66,7 @@ def load_controller_config(
 
         self.part_controllers.clear()
         self._action_split_indexes.clear()
+        self._action_split_joint_names.clear()
         self._init_controllers()
         self._validate_composite_controller_specific_config()
         self.setup_action_split_idx()
@@ -91,6 +93,7 @@ def setup_action_split_idx(self):
                 last_idx += self.grippers[part_name].dof
             else:
                 last_idx += controller.control_dim
+            self._action_split_joint_names[part_name] = controller.joint_names
             self._action_split_indexes[part_name] = (previous_idx, last_idx)
             previous_idx = last_idx
 
@@ -155,6 +158,52 @@ def action_limits(self):
                 low, high = np.concatenate([low, low_c]), np.concatenate([high, high_c])
         return low, high
 
+    def create_action_vector(self, action_dict: Dict[str, np.ndarray]) -> np.ndarray:
+        """
+        A helper function that creates the action vector given a dictionary
+        """
+        full_action_vector = np.zeros(self.action_limits[0].shape)
+        for (part_name, action_vector) in action_dict.items():
+            if part_name not in self._action_split_indexes:
+                ROBOSUITE_DEFAULT_LOGGER.debug(f"{part_name} is not specified in the action space")
+                continue
+            start_idx, end_idx = self._action_split_indexes[part_name]
+            if end_idx - start_idx == 0:
+                # skipping not controlled actions
+                continue
+            assert len(action_vector) == (end_idx - start_idx), ROBOSUITE_DEFAULT_LOGGER.error(
+                f"Action vecto(end_idx - start_idx) for {part_name} is not the correct size. Expected {end_idx} for {part_name}, got {len(action_vector)}"
+            )
+            full_action_vector[start_idx:end_idx] = action_vector
+        return full_action_vector
+
+    def print_action_info(self):
+        action_index_info = []
+        action_dim_info = []
+        joint_names_info = []
+        for part_name, (start_idx, end_idx) in self._action_split_indexes.items():
+            action_dim_info.append(f"{part_name}: {(end_idx - start_idx)} dim")
+            action_index_info.append(f"{part_name}: {start_idx}:{end_idx}")
+            joint_names_info.append(f"{part_name}: {self._action_split_joint_names[part_name]}")
+
+        action_dim_info_str = ", ".join(action_dim_info)
+        ROBOSUITE_DEFAULT_LOGGER.info(f"Action Dimensions: [{action_dim_info_str}]")
+
+        action_index_info_str = ", ".join(action_index_info)
+        ROBOSUITE_DEFAULT_LOGGER.info(f"Action Indices: [{action_index_info_str}]")
+
+        joint_names_info_str = ", ".join(joint_names_info)
+        ROBOSUITE_DEFAULT_LOGGER.info(f"Joint Names: [{joint_names_info_str}]")
+
+    def print_action_info_dict(self, name: str = ""):
+        info_dict = {}
+        info_dict["Action Dimension"] = self.action_limits[0].shape
+        info_dict["Action Split Indexes"] = dict(self._action_split_indexes)
+        info_dict["Action Split Joint Names"] = dict(self._action_split_joint_names)
+
+        info_dict_str = f"\nAction Info for {name}:\n\n{json.dumps(dict(info_dict), indent=4)}"
+        ROBOSUITE_DEFAULT_LOGGER.info(info_dict_str)
+
 
 @register_composite_controller
 class HybridMobileBase(CompositeController):

robosuite/controllers/config/default/composite/basic.json

@@ -3,43 +3,35 @@
     "body_parts": {
         "arms": {
             "right": {
-                "type": "OSC_POSE",
+                "type" : "JOINT_POSITION",
                 "input_max": 1,
                 "input_min": -1,
-                "output_max": [0.05, 0.05, 0.05, 0.5, 0.5, 0.5],
-                "output_min": [-0.05, -0.05, -0.05, -0.5, -0.5, -0.5],
-                "kp": 150,
-                "damping_ratio": 1,
-                "impedance_mode": "fixed",
-                "kp_limits": [0, 300],
-                "damping_ratio_limits": [0, 10],
-                "position_limits": null,
-                "orientation_limits": null,
-                "uncouple_pos_ori": true,
-                "input_type": "delta",
-                "input_ref_frame": "base",
+                "input_type": "absolute",
+                "output_max": 0.5,
+                "output_min": -0.5,
+                "kd": 200,
+                "kv": 200,
+                "kp": 1000,
+                "velocity_limits": [-1,1],
+                "kp_limits": [0, 1000],
                 "interpolation": null,
                 "ramp_ratio": 0.2,
                 "gripper": {
                     "type": "GRIP"
                 }
             },
             "left": {
-                "type": "OSC_POSE",
+                "type" : "JOINT_POSITION",
                 "input_max": 1,
                 "input_min": -1,
-                "output_max": [0.05, 0.05, 0.05, 0.5, 0.5, 0.5],
-                "output_min": [-0.05, -0.05, -0.05, -0.5, -0.5, -0.5],
-                "kp": 150,
-                "damping_ratio": 1,
-                "impedance_mode": "fixed",
-                "kp_limits": [0, 300],
-                "damping_ratio_limits": [0, 10],
-                "position_limits": null,
-                "orientation_limits": null,
-                "uncouple_pos_ori": true,
-                "input_type": "delta",
-                "input_ref_frame": "base",
+                "input_type": "absolute",
+                "output_max": 0.5,
+                "output_min": -0.5,
+                "kd": 200,
+                "kv": 200,
+                "kp": 1000,
+                "velocity_limits": [-1,1],
+                "kp_limits": [0, 1000],
                 "interpolation": null,
                 "ramp_ratio": 0.2,
                 "gripper": {
@@ -51,6 +43,7 @@
             "type" : "JOINT_POSITION",
             "input_max": 1,
             "input_min": -1,
+            "input_type": "absolute",
             "output_max": 0.5,
             "output_min": -0.5,
             "kd": 200,
@@ -65,6 +58,7 @@
             "type" : "JOINT_POSITION",
             "input_max": 1,
             "input_min": -1,
+            "input_type": "absolute",
             "output_max": 0.5,
             "output_min": -0.5,
             "kd": 200,
@@ -76,8 +70,8 @@
             "ramp_ratio": 0.2
         },
         "base": {
-            "type": "JOINT_VELOCITY",
-            "interpolation": "null"
+            "type" : "JOINT_VELOCITY",
+            "interpolation": null
         },
         "legs": {
             "type": "JOINT_POSITION",

robosuite/controllers/parts/gripper/simple_grip.py

@@ -107,6 +107,9 @@ def __init__(
         # initialize
         self.goal_qvel = None
 
+        self.joint_names = self.joint_index  # joint_index is a list of strings for some reason, at least for GR1. #TODO
+
+
     def set_goal(self, action, set_qpos=None):
         """
         Sets goal based on input @action. If self.impedance_mode is not "fixed", then the input will be parsed into the

robosuite/demos/demo_random_action.py

@@ -1,5 +1,6 @@
 import time
 
+from robosuite.controllers.composite.composite_controller_factory import load_composite_controller_config
 from robosuite.utils.input_utils import *
 
 MAX_FR = 25  # max frame rate for running simluation
@@ -40,6 +41,21 @@
     else:
         options["robots"] = choose_robots(exclude_bimanual=True)
 
+    # hardcoded GR1: TODO remove
+    options["robots"] = "GR1FixedLowerBody"
+    # Get controller config
+    controller_config = load_composite_controller_config(
+        controller="BASIC",
+    )
+
+    if controller_config["type"] == "WHOLE_BODY_MINK_IK":
+        # mink-speicific import. requires installing mink
+        from robosuite.examples.third_party_controller.mink_controller import WholeBodyMinkIK
+
+    # Create argument configuration
+    options["controller_configs"] = controller_config
+    # hardcoded GR1: TODO remove
+
     # initialize the task
     env = suite.make(
         **options,
@@ -60,6 +76,7 @@
         start = time.time()
 
         action = np.random.uniform(low, high)
+        env.robots[0].print_action_info_dict()  # TODO(remove)
         obs, reward, done, _ = env.step(action)
         env.render()