diff --git a/robosuite/controllers/config/default/composite/whole_body_ik.json b/robosuite/controllers/config/default/composite/whole_body_ik.json index 0f0caa4960..9e4fb73887 100644 --- a/robosuite/controllers/config/default/composite/whole_body_ik.json +++ b/robosuite/controllers/config/default/composite/whole_body_ik.json @@ -30,6 +30,7 @@ "type" : "JOINT_POSITION", "input_max": 1, "input_min": -1, + "input_type": "absolute", "output_max": 0.5, "output_min": -0.5, "kd": 200, @@ -47,6 +48,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 +67,7 @@ "type" : "JOINT_POSITION", "input_max": 1, "input_min": -1, + "input_type": "absolute", "output_max": 0.5, "output_min": -0.5, "kd": 200, @@ -79,6 +82,7 @@ "type" : "JOINT_POSITION", "input_max": 1, "input_min": -1, + "input_type": "absolute", "output_max": 0.5, "output_min": -0.5, "kd": 200, diff --git a/robosuite/controllers/config/default/composite/whole_body_mink_ik.json b/robosuite/controllers/config/default/composite/whole_body_mink_ik.json new file mode 100644 index 0000000000..7c96791656 --- /dev/null +++ b/robosuite/controllers/config/default/composite/whole_body_mink_ik.json @@ -0,0 +1,101 @@ +{ + "type": "WHOLE_BODY_MINK_IK", + "composite_controller_specific_configs": { + "ref_name": ["gripper0_right_grip_site", "gripper0_left_grip_site"], + "interpolation": null, + "actuation_part_names": ["torso", "head", "right", "left"], + "max_dq": 4, + "ik_pseudo_inverse_damping": 5e-2, + "ik_integration_dt": 1e-1, + "ik_input_type": "absolute", + "ik_input_ref_frame": "world", + "ik_input_rotation_repr": "axis_angle", + "verbose": false, + "ik_posture_weights": { + "robot0_torso_waist_yaw": 10.0, + "robot0_torso_waist_pitch": 10.0, + "robot0_torso_waist_roll": 200.0, + "robot0_l_shoulder_pitch": 4.0, + "robot0_r_shoulder_pitch": 4.0, + "robot0_l_shoulder_roll": 3.0, + "robot0_r_shoulder_roll": 3.0, + "robot0_l_shoulder_yaw": 2.0, + "robot0_r_shoulder_yaw": 2.0 + }, + "ik_hand_pos_cost": 1.0, + "ik_hand_ori_cost": 0.5, + "use_joint_angle_action_input": false + }, + "body_parts_controller_configs": { + "arms": { + "right": { + "type" : "JOINT_POSITION", + "input_max": 1, + "input_min": -1, + "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", + "use_action_scaling": false + } + }, + "left": { + "type" : "JOINT_POSITION", + "input_max": 1, + "input_min": -1, + "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", + "use_action_scaling": false + } + } + }, + "torso": { + "type" : "JOINT_POSITION", + "input_max": 1, + "input_min": -1, + "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 + }, + "head": { + "type" : "JOINT_POSITION", + "input_max": 1, + "input_min": -1, + "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 + } + } +} diff --git a/robosuite/devices/device.py b/robosuite/devices/device.py index fdd07bcf3c..32da777a0d 100644 --- a/robosuite/devices/device.py +++ b/robosuite/devices/device.py @@ -212,9 +212,7 @@ def get_arm_action(self, robot, arm, norm_delta, goal_update_mode="target"): pose_in_base = self.env.robots[0].composite_controller.joint_action_policy.transform_pose( src_frame_pose=pose_in_world, src_frame="world", # mocap pose is world coordinates - dst_frame=self.env.robots[0].composite_controller.composite_controller_specific_config.get( - "ik_input_ref_frame", "world" - ), + dst_frame=ref_frame, ) pos, ori = pose_in_base[:3, 3], pose_in_base[:3, :3] else: diff --git a/robosuite/examples/third_party_controller/mink_controller.py b/robosuite/examples/third_party_controller/mink_controller.py index fe9f187297..6d796ea6ee 100644 --- a/robosuite/examples/third_party_controller/mink_controller.py +++ b/robosuite/examples/third_party_controller/mink_controller.py @@ -17,6 +17,7 @@ from robosuite.models.grippers.gripper_model import GripperModel from robosuite.models.robots.robot_model import RobotModel from robosuite.utils.binding_utils import MjSim +from robosuite.utils.log_utils import ROBOSUITE_DEFAULT_LOGGER def update(self, q: Optional[np.ndarray] = None, update_idxs: Optional[np.ndarray] = None) -> None: @@ -424,10 +425,61 @@ class WholeBodyMinkIK(WholeBody): def __init__(self, sim: MjSim, robot_model: RobotModel, grippers: Dict[str, GripperModel]): super().__init__(sim, robot_model, grippers) + def _validate_composite_controller_specific_config(self) -> None: + # Check that all actuation_part_names exist in part_controllers + original_ik_controlled_parts = self.composite_controller_specific_config["actuation_part_names"] + self.valid_ik_controlled_parts = [] + valid_ref_names = [] + + assert ( + "ref_name" in self.composite_controller_specific_config + ), "The 'ref_name' key is missing from composite_controller_specific_config." + + for part in original_ik_controlled_parts: + if part in self.part_controllers: + self.valid_ik_controlled_parts.append(part) + else: + ROBOSUITE_DEFAULT_LOGGER.warning( + f"Part '{part}' specified in 'actuation_part_names' " + "does not exist in part_controllers. Removing ..." + ) + + # Update the configuration with only the valid parts + self.composite_controller_specific_config["actuation_part_names"] = self.valid_ik_controlled_parts + + # Loop through ref_names and validate against mujoco model + original_ref_names = self.composite_controller_specific_config.get("ref_name", []) + for ref_name in original_ref_names: + if ref_name in self.sim.model.site_names: # Check if the site exists in the mujoco model + valid_ref_names.append(ref_name) + else: + ROBOSUITE_DEFAULT_LOGGER.warning( + f"Reference name '{ref_name}' specified in configuration" + " does not exist in the mujoco model. Removing ..." + ) + + # Update the configuration with only the valid reference names + self.composite_controller_specific_config["ref_name"] = valid_ref_names + + # Check all the ik posture weights exist in the robot model + ik_posture_weights = self.composite_controller_specific_config.get("ik_posture_weights", {}) + valid_posture_weights = {} + for weight_name in ik_posture_weights: + if weight_name in self.robot_model.joints: + valid_posture_weights[weight_name] = ik_posture_weights[weight_name] + else: + ROBOSUITE_DEFAULT_LOGGER.warning( + f"Ik posture weight '{weight_name}' does not exist in the robot model. Removing ..." + ) + + # Update the configuration with only the valid posture weights + self.composite_controller_specific_config["ik_posture_weights"] = valid_posture_weights + def _init_joint_action_policy(self): joint_names: str = [] for part_name in self.composite_controller_specific_config["actuation_part_names"]: - joint_names += self.part_controllers[part_name].joint_names + if part_name in self.part_controllers: + joint_names += self.part_controllers[part_name].joint_names default_site_names: List[str] = [] for arm in ["right", "left"]: diff --git a/robosuite/utils/ik_utils.py b/robosuite/utils/ik_utils.py index a0565f088f..a4fc7bb9d5 100644 --- a/robosuite/utils/ik_utils.py +++ b/robosuite/utils/ik_utils.py @@ -298,7 +298,7 @@ def solve( # get the desired joint angles by integrating the desired joint velocities self.q_des = self.full_model_data.qpos[self.dof_ids].copy() - # mujoco.mj_integratePos(self.full_model, q_des, dq, self.integration_dt) + # mujoco.mj_integratePos(self.full_model, self.q_des, self.dq, self.integration_dt) self.q_des += self.dq * self.integration_dt # manually integrate q_des pre_clip_error = np.inf @@ -310,9 +310,7 @@ def solve( integrated_pos_np = np.array([integrated_pos[site] for site in integrated_pos]) pre_clip_error = np.linalg.norm(target_pos - integrated_pos_np) ROBOSUITE_DEFAULT_LOGGER.info(f"IK error before clipping based on joint ranges: {pre_clip_error}") - self.pre_clip_errors.append(pre_clip_error) - - self.debug_iter += 1 + # self.pre_clip_errors.append(pre_clip_error) # Set the control signal. np.clip(self.q_des, *self.full_model.jnt_range[self.dof_ids].T, out=self.q_des) @@ -324,4 +322,6 @@ def solve( post_clip_error = np.linalg.norm(target_pos - integrated_pos_np) ROBOSUITE_DEFAULT_LOGGER.info(f"IK error after clipping based on joint ranges: {post_clip_error}") + self.debug_iter += 1 + return self.q_des