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

action primitives bugfix for no op action IK controller #933

Merged
merged 9 commits into from
Oct 4, 2024
Merged

action primitives bugfix for no op action IK controller #933

Changes from all commits
Commits
Show all changes
9 commits
Select commit Hold shift + click to select a range
763d846
install setup file minor update
Sep 17, 2024
1dfcf88
setup removed
Oct 3, 2024
d65d120
action primitives roboarm bugfix
Oct 3, 2024
8f77285
Merge branch 'og-develop' into primitive_bugfix
yyf20001230 Oct 4, 2024
6c6fb7d
temp fix
Oct 4, 2024
aee049d
Merge branch 'primitive_bugfix' of https://github.com/StanfordVL/Omni…
Oct 4, 2024
af4f4a5
[pre-commit.ci] auto fixes from pre-commit.com hooks
pre-commit-ci[bot] Oct 4, 2024
eeb9258
Dont compute expensive delta orn if not necessary
cgokmen Oct 4, 2024
f490d1e
Merge branch 'og-develop' into primitive_bugfix
cgokmen Oct 4, 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
24 changes: 17 additions & 7 deletions omnigibson/action_primitives/starter_semantic_action_primitives.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -36,7 +36,7 @@
from omnigibson.robots.locomotion_robot import LocomotionRobot
from omnigibson.robots.manipulation_robot import ManipulationRobot
from omnigibson.tasks.behavior_task import BehaviorTask
from omnigibson.utils.control_utils import FKSolver, IKSolver
from omnigibson.utils.control_utils import FKSolver, IKSolver, orientation_error
from omnigibson.utils.grasping_planning_utils import get_grasp_poses_for_object_sticky, get_grasp_position_for_open
from omnigibson.utils.motion_planning_utils import (
detect_robot_collision_in_sim,
Expand Down Expand Up @@ -1462,13 +1462,23 @@ def _empty_action(self):
# if desired arm targets are available, generate an action that moves the arms to the saved pose targets
if name in self._arm_targets:
if isinstance(controller, InverseKinematicsController):
target_pose = self._arm_targets[name]
target_orn_axisangle = target_pose[1]
current_pose = self._get_pose_in_robot_frame(
(self.robot.get_eef_position(self.arm), self.robot.get_eef_orientation(self.arm))
arm = name.replace("arm_", "")
target_pos, target_orn_axisangle = self._arm_targets[name]
current_pos, current_orn = self._get_pose_in_robot_frame(
(self.robot.get_eef_position(arm), self.robot.get_eef_orientation(arm))
)
delta_pos = target_pose[0] - current_pose[0]
partial_action = th.cat((delta_pos, target_orn_axisangle))
delta_pos = target_pos - current_pos
if controller.mode == "pose_delta_ori":
delta_orn = orientation_error(
T.quat2mat(T.axisangle2quat(target_orn_axisangle)), T.quat2mat(current_orn)
)
partial_action = th.cat((delta_pos, delta_orn))
elif controller.mode in "pose_absolute_ori":
partial_action = th.cat((delta_pos, target_orn_axisangle))
elif controller.mode == "absolute_pose":
partial_action = th.cat((target_pos, target_orn_axisangle))
else:
raise ValueError("Unexpected IK control mode")
else:
target_joint_pos = self._arm_targets[name]
current_joint_pos = self.robot.get_joint_positions()[self._manipulation_control_idx]
Expand Down
Loading