Skip to content

Commit

Permalink
Fixes w/ Sujay
Browse files Browse the repository at this point in the history
  • Loading branch information
@cgokmen
cgokmen committed Oct 26, 2023
1 parent b24a57e commit e170158
Show file tree
Hide file tree
Showing 5 changed files with 61 additions and 122 deletions.
113 changes: 40 additions & 73 deletions omnigibson/action_primitives/starter_semantic_action_primitives.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -135,11 +135,12 @@ def _assemble_robot_copy(self):
robot_description_path=self.robot.robot_arm_descriptor_yamls[fk_descriptor],
robot_urdf_path=self.robot.urdf_path,
)

arm_links = self.robot.manipulation_link_names

if TORSO_FIXED:
assert self.arm == "left", "Fixed torso mode only supports left arm!"
joint_control_idx = self.robot.arm_cotnrol_idx["left"]
joint_control_idx = self.robot.arm_control_idx["left"]
joint_pos = np.array(self.robot.get_joint_positions()[joint_control_idx])
else:
joint_combined_idx = np.concatenate([self.robot.trunk_control_idx, self.robot.arm_control_idx[fk_descriptor]])
Expand Down Expand Up @@ -182,7 +183,7 @@ def _construct_disabled_collision_pairs(self):
self.disabled_collision_pairs_dict[mesh.GetPrimPath().pathString] += all_meshes
# Filter out collision pairs of meshes part of disabled collision pairs
else:
for pair in self.robot.primitive_disabled_collision_pairs:
for pair in self.robot.disabled_collision_pairs:
link_1 = pair[0]
link_2 = pair[1]
if link_1 in robot_meshes_copy.keys() and link_2 in robot_meshes_copy.keys():
Expand Down Expand Up @@ -484,7 +485,7 @@ def _open_or_close(self, obj, should_open):

grasp_pose, target_poses, object_direction, relevant_joint, grasp_required, pos_change = grasp_data
if abs(pos_change) < 0.1:
print("Yaw change is small and done,", pos_change)
indented_print("Yaw change is small and done,", pos_change)
return

# Prepare data for the approach later.
Expand All @@ -494,7 +495,6 @@ def _open_or_close(self, obj, should_open):
# If the grasp pose is too far, navigate
yield from self._navigate_if_needed(obj, pose_on_obj=grasp_pose)

print("MOVE HAND")
yield from self._move_hand(grasp_pose, stop_if_stuck=True)

# We can pre-grasp in sticky grasping mode only for opening
Expand All @@ -503,20 +503,18 @@ def _open_or_close(self, obj, should_open):

# Since the grasp pose is slightly off the object, we want to move towards the object, around 5cm.
# It's okay if we can't go all the way because we run into the object.
yield from self._navigate_if_needed(obj, pose_on_obj=approach_pose) #, check_joint=check_joint)
yield from self._navigate_if_needed(obj, pose_on_obj=approach_pose)

if should_open:
yield from self._move_hand_direct_cartesian(approach_pose, ignore_failure=False, stop_on_contact=True, stop_if_stuck=True)
else:
print("go to approach pose")
yield from self._move_hand_direct_cartesian(approach_pose, ignore_failure=False, stop_if_stuck=True)

# Step once to update
empty_action = self._empty_action()
yield self._postprocess_action(empty_action)

for i, target_pose in enumerate(target_poses):
print(f"go to target pose {i}")
yield from self._move_hand_direct_cartesian(target_pose, ignore_failure=False, stop_if_stuck=True)

# Moving to target pose often fails. This might leave the robot's motors with torques that
Expand All @@ -535,7 +533,7 @@ def _open_or_close(self, obj, should_open):
yield from self._move_base_backward()

except ActionPrimitiveError as e:
print(e)
indented_print(e)
if should_open:
yield from self._execute_release()
yield from self._move_base_backward()
Expand All @@ -549,6 +547,7 @@ def _open_or_close(self, obj, should_open):
{"target object": obj.name, "is it currently open": obj.states[object_states.Open].get_value()},
)

# TODO: Figure out how to generalize out of this "backing out" behavior.
def _move_base_backward(self, steps=5, speed=0.2):
"""
Yields action for the robot to move base so the eef is in the target pose using the planner
Expand Down Expand Up @@ -962,10 +961,6 @@ def _add_linearly_interpolated_waypoints(self, plan, max_inter_dist):
return interpolated_plan

def _compute_delta_command(self, controller_name, diff_joint_pos, gain=1.0, min_action=0.0):
controller = self.robot._controllers[controller_name]
control_limits = controller._control_limits[controller.control_type]
gain = 1.0
min_action = 0.0
# for joints not within thresh, set a minimum action value
# for joints within thres, set action to zero
delta_action = gain * diff_joint_pos
Expand Down Expand Up @@ -1369,6 +1364,7 @@ def _reset_hand(self):
yield from self._move_hand_direct_joint(reset_pose, control_idx, ignore_failure=True)

def _get_reset_eef_pose(self):
# TODO: Add support for Fetch
if self.robot_model == "Tiago":
return np.array([0.28493954, 0.37450749, 1.1512334]), np.array([-0.21533823, 0.05361032, -0.08631776, 0.97123871])
else:
Expand All @@ -1393,37 +1389,7 @@ def _get_reset_joint_pos(self):
0.05, # gripper
]
)

# reset_pose_tiago = np.array([
# -1.78029833e-04,
# 3.20231302e-05,
# -1.85759447e-07,
# -1.16488536e-07,
# 4.55182843e-08,
# 2.36128806e-04,
# 0.15,
# 0.94,
# -1.1,
# 0.0,
# -0.9,
# 1.47,
# 0.0,
# 2.1,
# 2.71,
# 1.5,
# 1.71,
# 1.3,
# -1.57,
# -1.4,
# 1.39,
# 0.0,
# 0.0,
# 0.045,
# 0.045,
# 0.045,
# 0.045,
# ])


reset_pose_tiago = np.array([
-1.78029833e-04,
3.20231302e-05,
Expand Down Expand Up @@ -1566,13 +1532,13 @@ def _navigate_to_pose_direct(self, pose_2d, low_precision=False):
diff_pos = end_pose[0] - self.robot.get_position()
intermediate_pose = (end_pose[0], T.euler2quat([0, 0, np.arctan2(diff_pos[1], diff_pos[0])]))
body_intermediate_pose = self._get_pose_in_robot_frame(intermediate_pose)
diff_yaw = T.wrap_angle(T.quat2euler(body_intermediate_pose[1])[2])
diff_yaw = T.quat2euler(body_intermediate_pose[1])[2]
if abs(diff_yaw) > DEFAULT_ANGLE_THRESHOLD:
yield from self._rotate_in_place(intermediate_pose, angle_threshold=DEFAULT_ANGLE_THRESHOLD)
else:
action = self._empty_action(arm_pose_to_keep=initial_eef_pos)
if self.robot_model == "Tiago":
direction_vec = body_target_pose[0][:2] / (np.linalg.norm(body_target_pose[0][:2]) * 5)
direction_vec = body_target_pose[0][:2] / np.linalg.norm(body_target_pose[0][:2]) * KP_LIN_VEL
base_action = [direction_vec[0], direction_vec[1], 0.0]
action[self.robot.controller_action_idx["base"]] = base_action
else:
Expand All @@ -1597,7 +1563,7 @@ def _rotate_in_place(self, end_pose, angle_threshold = DEFAULT_ANGLE_THRESHOLD):
np.array or None: Action array for one step for the robot to rotate or None if it is done rotating
"""
body_target_pose = self._get_pose_in_robot_frame(end_pose)
diff_yaw = T.wrap_angle(T.quat2euler(body_target_pose[1])[2])
diff_yaw = T.quat2euler(body_target_pose[1])[2]
initial_eef_pos = self.robot.get_eef_position()

while abs(diff_yaw) > angle_threshold:
Expand All @@ -1611,7 +1577,7 @@ def _rotate_in_place(self, end_pose, angle_threshold = DEFAULT_ANGLE_THRESHOLD):
yield self._postprocess_action(action)

body_target_pose = self._get_pose_in_robot_frame(end_pose)
diff_yaw = T.wrap_angle(T.quat2euler(body_target_pose[1])[2])
diff_yaw = T.quat2euler(body_target_pose[1])[2]

empty_action = self._empty_action(arm_pose_to_keep=initial_eef_pos)
yield self._postprocess_action(empty_action)
Expand Down Expand Up @@ -1680,31 +1646,31 @@ def _sample_position_on_aabb_side(target_obj):
face_max = face_center + face_vertical_half_extent + face_lateral_half_extent
return np.random.uniform(face_min, face_max)

def _sample_pose_in_room(self, room: str):
"""
Returns a pose for the robot within in the room where the robot is not in collision with anything
Args:
room (str): Name of room
Returns:
2-tuple:
- 3-array: (x,y,z) Position in the world frame
- 4-array: (x,y,z,w) Quaternion orientation in the world frame
"""
# TODO(MP): Bias the sampling near the agent.
for _ in range(MAX_ATTEMPTS_FOR_SAMPLING_POSE_IN_ROOM):
_, pos = self.env.scene.get_random_point_by_room_instance(room)
yaw = np.random.uniform(-np.pi, np.pi)
pose = (pos[0], pos[1], yaw)
if self._test_pose(pose):
return pose

raise ActionPrimitiveError(
ActionPrimitiveError.Reason.SAMPLING_ERROR,
"Could not find valid position in the given room to travel to",
{"room": room}
)
# def _sample_pose_in_room(self, room: str):
# """
# Returns a pose for the robot within in the room where the robot is not in collision with anything

# Args:
# room (str): Name of room

# Returns:
# 2-tuple:
# - 3-array: (x,y,z) Position in the world frame
# - 4-array: (x,y,z,w) Quaternion orientation in the world frame
# """
# # TODO(MP): Bias the sampling near the agent.
# for _ in range(MAX_ATTEMPTS_FOR_SAMPLING_POSE_IN_ROOM):
# _, pos = self.env.scene.get_random_point_by_room_instance(room)
# yaw = np.random.uniform(-np.pi, np.pi)
# pose = (pos[0], pos[1], yaw)
# if self._test_pose(pose):
# return pose

# raise ActionPrimitiveError(
# ActionPrimitiveError.Reason.SAMPLING_ERROR,
# "Could not find valid position in the given room to travel to",
# {"room": room}
# )

def _sample_pose_with_object_and_predicate(self, predicate, held_obj, target_obj, near_poses=None, near_poses_threshold=None):
"""
Expand Down Expand Up @@ -1751,7 +1717,8 @@ def _sample_pose_with_object_and_predicate(self, predicate, held_obj, target_obj
{"target object": target_obj.name, "object in hand": held_obj.name, "relation": pred_map[predicate]},
)

def _test_pose(self, pose_2d, context, pose_on_obj=None, check_joint=None):
# TODO: Why do we need to pass in the context here?
def _test_pose(self, pose_2d, context, pose_on_obj=None):
"""
Determines whether the robot can reach the pose on the object and is not in collision at the specified 2d pose
Expand Down
12 changes: 0 additions & 12 deletions omnigibson/robots/fetch.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -403,18 +403,6 @@ def gripper_control_idx(self):

@property
def disabled_collision_pairs(self):
return [
["torso_lift_link", "shoulder_lift_link"],
["torso_lift_link", "torso_fixed_link"],
]

@property
def primitive_disabled_collision_pairs(self):
"""
Returns:
Array of arrays: Disabled collision pairs for necessary for primitives. Including these in disabled_collision_pairs
throws an error. When bug is fixed, these can be merged with disabled_collision_pairs.
"""
return [
["torso_lift_link", "shoulder_lift_link"],
["torso_lift_link", "torso_fixed_link"],
Expand Down
8 changes: 0 additions & 8 deletions omnigibson/robots/tiago.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -605,14 +605,6 @@ def disabled_collision_pairs(self):
['arm_right_tool_link', 'wrist_right_ft_tool_link']
]

@property
def primitive_disabled_collision_pairs(self):
"""
Returns:
Array of arrays: This property in Fetch exists due to a bug. To keep consistency, this exists here. When the bug is fixed in Fetch, this property can be removed from here.
"""
return self.disabled_collision_pairs

@property
def manipulation_link_names(self):
return [
Expand Down
37 changes: 21 additions & 16 deletions omnigibson/utils/motion_planning_utils.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -7,6 +7,20 @@
from omnigibson.utils.control_utils import IKSolver
from pxr import PhysicsSchemaTools, Gf


def _wrap_angle(theta):
""""
Converts an angle to the range [-pi, pi).
Args:
theta (float): angle in radians
Returns:
float: angle in radians in range [-pi, pi)
"""
return (theta + np.pi) % (2 * np.pi) - np.pi


def plan_base_motion(
robot,
end_conf,
Expand Down Expand Up @@ -69,7 +83,7 @@ def checkMotion(self, s1, s2):

@staticmethod
def is_valid_rotation(si, start_conf, final_orientation):
diff = T.wrap_angle(final_orientation - start_conf[2])
diff = _wrap_angle(final_orientation - start_conf[2])
direction = np.sign(diff)
diff = abs(diff)
num_points = ceil(diff / ANGLE_DIFF) + 1
Expand All @@ -93,7 +107,7 @@ def create_state(space, x, y, yaw):
state = ob.State(space)
state().setX(x)
state().setY(y)
state().setYaw(T.wrap_angle(yaw))
state().setYaw(_wrap_angle(yaw))
return state

def state_valid_fn(q):
Expand Down Expand Up @@ -165,10 +179,10 @@ def remove_unnecessary_rotations(path):
planner = ompl_geo.RRT(si)
ss.setPlanner(planner)

start = create_state(space, start_conf[0], start_conf[1], T.wrap_angle(start_conf[2]))
start = create_state(space, start_conf[0], start_conf[1], start_conf[2])
print(start)

goal = create_state(space, end_conf[0], end_conf[1], T.wrap_angle(end_conf[2]))
goal = create_state(space, end_conf[0], end_conf[1], end_conf[2])
print(goal)

ss.setStartAndGoalStates(start, goal)
Expand Down Expand Up @@ -337,7 +351,6 @@ def plan_arm_motion_ik(
ik_solver = IKSolver(
robot_description_path=robot_description_path,
robot_urdf_path=robot.urdf_path,
# default_joint_pos=robot.get_joint_positions()[joint_control_idx],
default_joint_pos=robot.default_joint_pos[joint_control_idx],
eef_name=robot.eef_link_names[robot.default_arm],
)
Expand All @@ -350,11 +363,7 @@ def state_valid_fn(q):
target_quat=T.axisangle2quat(eef_pose[3:]),
max_iterations=1000,
)
# ik_solver.solve(
# target_pos=eef_pose[:3],
# target_quat=T.axisangle2quat(eef_pose[3:]),
# max_iterations=1000,
# )

if control_joint_pos is None:
return False
joint_pos[control_idx_in_joint_pos] = control_joint_pos
Expand Down Expand Up @@ -434,13 +443,10 @@ def set_base_and_detect_collision(context, pose):
robot_copy = context.robot_copy
robot_copy_type = context.robot_copy_type

translation = pose[0]
orientation = pose[1]
# context.robot_copy.prim.set_local_poses(np.array([translation]), np.array([orientation]))
translation = Gf.Vec3d(*np.array(translation, dtype=float))
translation = Gf.Vec3d(*np.array(pose[0], dtype=float))
robot_copy.prims[robot_copy_type].GetAttribute("xformOp:translate").Set(translation)

orientation = np.array(orientation, dtype=float)[[3, 0, 1, 2]]
orientation = np.array(pose[1], dtype=float)[[3, 0, 1, 2]]
robot_copy.prims[robot_copy_type].GetAttribute("xformOp:orient").Set(Gf.Quatd(*orientation))

return detect_robot_collision(context)
Expand Down Expand Up @@ -495,7 +501,6 @@ def detect_robot_collision(context):

def overlap_callback(hit):
nonlocal valid_hit
nonlocal mesh_path

valid_hit = hit.rigid_body not in context.disabled_collision_pairs_dict[mesh_path]

Expand Down
13 changes: 0 additions & 13 deletions omnigibson/utils/transform_utils.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -533,19 +533,6 @@ def quat2euler(quat):
"""
return R.from_quat(quat).as_euler("xyz")

def wrap_angle(theta, lower=-np.pi):
""""
Converts an angle to the range [lower, lower + 2*pi).
Defaults to wrapping to [-pi, pi).
Args:
theta (float): angle in radians
Returns:
float: angle in radians in range [lower, lower + 2*pi)
"""
return (theta - lower) % (2 * np.pi) + lower

def pose_in_A_to_pose_in_B(pose_A, pose_A_in_B):
"""
Converts a homogenous matrix corresponding to a point C in frame A
Expand Down

0 comments on commit e170158

Please sign in to comment.