diff --git a/robosuite/controllers/composite/composite_controller.py b/robosuite/controllers/composite/composite_controller.py index b832827c38..f8aa33523a 100644 --- a/robosuite/controllers/composite/composite_controller.py +++ b/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): diff --git a/robosuite/controllers/config/default/composite/basic.json b/robosuite/controllers/config/default/composite/basic.json index 90b9debb5f..6077a58498 100644 --- a/robosuite/controllers/config/default/composite/basic.json +++ b/robosuite/controllers/config/default/composite/basic.json @@ -3,21 +3,17 @@ "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": { @@ -25,21 +21,17 @@ } }, "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", diff --git a/robosuite/controllers/parts/gripper/simple_grip.py b/robosuite/controllers/parts/gripper/simple_grip.py index 359b5ebcc7..13b2ae8166 100644 --- a/robosuite/controllers/parts/gripper/simple_grip.py +++ b/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 diff --git a/robosuite/demos/demo_random_action.py b/robosuite/demos/demo_random_action.py index 6ce751c1c9..0f09f4d596 100644 --- a/robosuite/demos/demo_random_action.py +++ b/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()