Skip to content

Commit

Permalink
Merge pull request #550 from Dingry/whole_body_ik_fix
Browse files Browse the repository at this point in the history 
[fix] fix whole_body_ik config and add a default whole_body_mink_ik config
  • Loading branch information
@kevin-thankyou-lin
kevin-thankyou-lin authored Nov 6, 2024
2 parents b81bde4 + 4ca1e44 commit fc550c0
Show file tree
Hide file tree
Showing 5 changed files with 163 additions and 8 deletions.
4 changes: 4 additions & 0 deletions robosuite/controllers/config/default/composite/whole_body_ik.json
View file
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
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
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
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
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

0 comments on commit fc550c0

Please sign in to comment.