p04798526
/
OmniGibson
mirror of https://github.com/StanfordVL/OmniGibson.git
1
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

Address comments

This commit is contained in:
Wensi (Vince) Ai 2024-03-15 23:22:30 -07:00
parent 9c6c79bd66
commit 5a8aae703f
10 changed files with 36 additions and 58 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

13
omnigibson/examples/teleoperation/robot_teleoperate_demo.py
View File

@ -1,17 +1,8 @@
"""
Example script for using external devices to teleoperate a robot.
"""
try:
from mediapipe import solutions
except ModuleNotFoundError:
pass
import omnigibson as og
from omnigibson.utils.ui_utils import choose_from_options
from omnigibson.utils.teleop_utils import TeleopSystem
from telemoma.utils.camera_utils import RealSenseCamera
from telemoma.configs.base_config import teleop_config
ROBOTS = {
"FrankaPanda": "Franka Emika Panda (default)",
@ -26,6 +17,10 @@ TELEOP_METHOD = {
}
def main():
from omnigibson.utils.teleop_utils import TeleopSystem
from telemoma.utils.camera_utils import RealSenseCamera
from telemoma.configs.base_config import teleop_config
robot_name = choose_from_options(options=ROBOTS, name="robot")
arm_teleop_method = choose_from_options(options=TELEOP_METHOD, name="robot arm teleop method")
if robot_name != "FrankaPanda":

3
omnigibson/robots/behavior_robot.py
View File

@ -5,7 +5,6 @@ import numpy as np
import os
from scipy.spatial.transform import Rotation as R
from typing import List, Tuple, Iterable
from telemoma.human_interface.teleop_core import TeleopAction
import omnigibson as og
import omnigibson.lazy as lazy
@ -432,7 +431,7 @@ class BehaviorRobot(ManipulationRobot, LocomotionRobot, ActiveCameraRobot):
self._part_is_in_contact[hand_name] = len(self.eef_links[hand_name].contact_list()) > 0 \
or np.any([len(finger.contact_list()) > 0 for finger in self.finger_links[hand_name]])
def teleop_data_to_action(self, teleop_action: TeleopAction) -> np.ndarray:
def teleop_data_to_action(self, teleop_action) -> np.ndarray:
"""
Generates an action for the BehaviorRobot to perform based on teleop action data dict.

3
omnigibson/robots/manipulation_robot.py
View File

@ -2,7 +2,6 @@ from abc import abstractmethod
from collections import namedtuple
import numpy as np
import networkx as nx
from telemoma.human_interface.teleop_core import TeleopAction
import omnigibson as og
import omnigibson.lazy as lazy
@ -1474,7 +1473,7 @@ class ManipulationRobot(BaseRobot):
"""
return {arm: np.array([0, 0, 0, 1]) for arm in self.arm_names}
def teleop_data_to_action(self, teleop_action: TeleopAction) -> np.ndarray:
def teleop_data_to_action(self, teleop_action) -> np.ndarray:
"""
Generate action data from teleoperation action data
NOTE: This implementation only supports IK/OSC controller for arm and MultiFingerGripperController for gripper.

3
omnigibson/robots/robot_base.py
View File

@ -2,7 +2,6 @@ from abc import abstractmethod
from copy import deepcopy
import numpy as np
import matplotlib.pyplot as plt
from telemoma.human_interface.teleop_core import TeleopAction
from omnigibson.macros import create_module_macros
from omnigibson.sensors import create_sensor, SENSOR_PRIMS_TO_SENSOR_CLS, ALL_SENSOR_MODALITIES, VisionSensor, ScanSensor
@ -480,7 +479,7 @@ class BaseRobot(USDObject, ControllableObject, GymObservable):
"""
return self._reset_joint_pos_aabb_extent
def teleop_data_to_action(self, teleop_action: TeleopAction) -> np.ndarray:
def teleop_data_to_action(self, teleop_action) -> np.ndarray:
"""
Generate action data from teleoperation action data
Args:

4
omnigibson/robots/tiago.py
View File

@ -1,7 +1,5 @@
import os
import numpy as np
from telemoma.human_interface.teleop_core import TeleopAction
import omnigibson as og
import omnigibson.lazy as lazy
from omnigibson.macros import gm
@ -763,7 +761,7 @@ class Tiago(ManipulationRobot, LocomotionRobot, ActiveCameraRobot):
def eef_usd_path(self):
return {arm: os.path.join(gm.ASSET_PATH, "models/tiago/tiago_dual_omnidirectional_stanford/tiago_eef.usd") for arm in self.arm_names}
def teleop_data_to_action(self, teleop_action: TeleopAction) -> np.ndarray:
def teleop_data_to_action(self, teleop_action) -> np.ndarray:
action = ManipulationRobot.teleop_data_to_action(self, teleop_action)
action[self.base_action_idx] = teleop_action.base * 0.1
return action

3
omnigibson/robots/two_wheel_robot.py
View File

@ -1,7 +1,6 @@
from abc import abstractmethod
import gym
import numpy as np
from telemoma.human_interface.teleop_core import TeleopAction
from omnigibson.controllers import DifferentialDriveController
from omnigibson.robots.locomotion_robot import LocomotionRobot
@ -150,7 +149,7 @@ class TwoWheelRobot(LocomotionRobot):
classes.add("TwoWheelRobot")
return classes
def teleop_data_to_action(self, teleop_action: TeleopAction) -> np.ndarray:
def teleop_data_to_action(self, teleop_action) -> np.ndarray:
"""
Generate action data from teleoperation action data
NOTE: This implementation only supports DifferentialDriveController.

17
omnigibson/utils/teleop_utils.py
View File

@ -9,10 +9,13 @@ from omnigibson.macros import create_module_macros
from omnigibson.objects import USDObject
from omnigibson.robots.robot_base import BaseRobot
from telemoma.human_interface.teleop_core import TeleopAction, TeleopObservation
from telemoma.human_interface.teleop_policy import TeleopPolicy
from telemoma.utils.general_utils import AttrDict
from telemoma.configs.base_config import teleop_config
try:
from telemoma.human_interface.teleop_core import TeleopAction, TeleopObservation
from telemoma.human_interface.teleop_policy import TeleopPolicy
from telemoma.utils.general_utils import AttrDict
from telemoma.configs.base_config import teleop_config
except ImportError as e:
raise e from ValueError("For teleoperation, install telemoma by running 'pip install telemoma'")
m = create_module_macros(module_path=__file__)
m.movement_speed = 0.2 # the speed of the robot base movement
@ -23,7 +26,7 @@ class TeleopSystem(TeleopPolicy):
"""
def __init__(self, config: AttrDict, robot: BaseRobot, show_control_marker: bool = False) -> None:
"""
Initializes the Teleoperatin System
Initializes the Teleoperation System
Args:
config (AttrDict): configuration dictionary
robot (BaseRobot): the robot that will be controlled.
@ -262,14 +265,14 @@ class OVXRSystem(TeleopSystem):
# update right hand related info
for arm_name, arm in zip(["left", "right"], self.robot_arms):
if arm in self.controllers:
self.teleop_action[arm_name] = np.r_[
self.teleop_action[arm_name] = np.concatenate((
self.raw_data["transforms"]["controllers"][arm][0],
T.quat2euler(T.quat_multiply(
self.raw_data["transforms"]["controllers"][arm][1],
self.robot.teleop_rotation_offset[self.robot.arm_names[self.robot_arms.index(arm)]]
)),
[self.raw_data["button_data"][arm]["axis"]["trigger"]]
]
))
self.teleop_action.is_valid[arm_name] = self._is_valid_transform(self.raw_data["transforms"]["controllers"][arm])
else:
self.teleop_action.is_valid[arm_name] = False

3
requirements-dev.txt
View File

@ -8,4 +8,5 @@ mkdocs-material-extensions
mkdocstrings[python]
mkdocs-section-index
mkdocs-literate-nav
pynvml
pynvml
telemoma~=0.1.2

1
setup.py
View File

@ -45,7 +45,6 @@ setup(
"click~=8.1.3",
"aenum~=3.1.15",
"rtree~=1.2.0",
"telemoma~=0.1.2"
],
tests_require=[],
python_requires=">=3",

44
tests/test_robot_teleoperation.py
View File

@ -1,11 +1,9 @@
import pytest
import omnigibson as og
import numpy as np
from omnigibson.macros import gm
from omnigibson.utils.teleop_utils import TeleopSystem
from omnigibson.utils.transform_utils import quat2euler, euler2quat
from telemoma.human_interface.teleop_core import TeleopAction
from omnigibson.utils.transform_utils import quat2euler
@pytest.mark.skip(reason="test is broken")
def test_teleop():
cfg = {
"env": {"action_timestep": 1 / 60., "physics_timestep": 1 / 120.},
@ -17,8 +15,7 @@ def test_teleop():
"controller_config": {
"arm_0": {
"name": "InverseKinematicsController",
"mode": "pose_absolute_ori",
"motor_type": "position"
"command_input_limits": None,
},
}
}
@ -26,49 +23,38 @@ def test_teleop():
}
# Make sure sim is stopped
og.sim.stop()
if og.sim is not None:
og.sim.stop()
# Make sure GPU dynamics are enabled (GPU dynamics needed for cloth) and no flatcache
gm.ENABLE_OBJECT_STATES = True
gm.USE_GPU_DYNAMICS = True
gm.USE_GPU_DYNAMICS = False
gm.ENABLE_FLATCACHE = False
# Create the environment
env = og.Environment(configs=cfg)
robot = env.robots[0]
env.reset()
teleop_system = TeleopSystem(robot=robot, show_control_marker=False)
teleop_action = TeleopAction()
start_base_pose = robot.get_position_orientation()
start_eef_pose = robot.links[robot.eef_link_names[robot.default_arm]].get_position_orientation()
# test moving robot arm
teleop_system.teleop_data.robot_attached = True
teleop_system.reset_transform_mapping()
target_eef_pos = start_eef_pose[0] + np.array([0.05, 0.05, 0])
target_eef_orn = euler2quat(quat2euler(start_eef_pose[1]) + np.array([0.5, 0.5, 0]))
teleop_system.teleop_data.is_valid["right"] = True
teleop_system.teleop_data.transforms["base"] = np.zeros(4)
teleop_system.teleop_data.transforms["right"] = (target_eef_pos, target_eef_orn)
teleop_action.right = np.concatenate(([0.01], np.zeros(6)))
for _ in range(50):
action = teleop_system.teleop_data_to_action()
action = robot.teleop_data_to_action(teleop_action)
env.step(action)
cur_eef_pose = robot.links[robot.eef_link_names[robot.default_arm]].get_position_orientation()
cur_base_pose = robot.get_position_orientation()
assert np.allclose(cur_base_pose[0], start_base_pose[0], atol=1e-2), "base pos not in starting place"
assert np.allclose(cur_base_pose[1], start_base_pose[1], atol=1e-2), "base orn not in starting place"
assert np.allclose(cur_eef_pose[0], target_eef_pos, atol=1e-2), "eef pos not in target place"
assert np.allclose(cur_eef_pose[1], target_eef_orn, atol=1e-2) or np.allclose(cur_eef_pose[1], -target_eef_orn, atol=1e-2), \
"eef orn not in target place"
assert cur_eef_pose[0][0] - start_eef_pose[0][0] > 0.02, "Robot arm not moving forward"
# test moving robot base
teleop_system.teleop_data.transforms["right"] = cur_eef_pose
teleop_system.teleop_data.transforms["base"] = np.array([0.02, 0, 0, 0.02])
teleop_action.right = np.zeros(7)
teleop_action.base = np.array([0.1, 0, 0.1])
for _ in range(50):
action = teleop_system.teleop_data_to_action()
action = robot.teleop_data_to_action(teleop_action)
env.step(action)
cur_base_pose = robot.get_position_orientation()
assert cur_base_pose[0][0] > start_base_pose[0][0], "robot base not moving forward"
assert quat2euler(cur_base_pose[1])[2] > quat2euler(start_base_pose[1])[2], "robot base not rotating counter-clockwise"
assert cur_base_pose[0][0] - start_base_pose[0][0] > 0.02, "robot base not moving forward"
assert quat2euler(cur_base_pose[1])[2] - quat2euler(start_base_pose[1])[2] > 0.02, "robot base not rotating counter-clockwise"
# Clear the sim
og.sim.clear()