Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

[fix] fix whole_body_ik config and add a default whole_body_mink_ik config #550

Merged
merged 2 commits into from
Nov 6, 2024
Merged

[fix] fix whole_body_ik config and add a default whole_body_mink_ik config #550

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
2 commits
Select commit Hold shift + click to select a range
c5871fc
[fix] fix whole_body_ik config and add whole_body_mink_ik example
Dingry Nov 6, 2024
4ca1e44
[fix] fix format
Dingry Nov 6, 2024
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
4 changes: 4 additions & 0 deletions robosuite/controllers/config/default/composite/whole_body_ik.json
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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,
Expand All @@ -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,
Expand All @@ -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,
Expand All @@ -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,
Expand Down
101 changes: 101 additions & 0 deletions robosuite/controllers/config/default/composite/whole_body_mink_ik.json
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -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
}
}
}
4 changes: 1 addition & 3 deletions robosuite/devices/device.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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:
Expand Down
54 changes: 53 additions & 1 deletion robosuite/examples/third_party_controller/mink_controller.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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:
Expand Down Expand Up @@ -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"]:
Expand Down
8 changes: 4 additions & 4 deletions robosuite/utils/ik_utils.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand All @@ -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)
Expand All @@ -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
Loading