Skip to content

Commit

Permalink
Update draped demo
Browse files Browse the repository at this point in the history
  • Loading branch information
@hang-yin
hang-yin committed Oct 26, 2024
1 parent 34e7937 commit 8814314
Showing 1 changed file with 75 additions and 49 deletions.
124 changes: 75 additions & 49 deletions omnigibson/examples/object_states/draped_demo.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -6,6 +6,7 @@
from omnigibson.object_states import Draped
from omnigibson.utils.constants import PrimType
from omnigibson.utils.python_utils import multi_dim_linspace
from omnigibson.utils.ui_utils import KeyboardRobotController

# Make sure object states and GPU dynamics are enabled (GPU dynamics needed for cloth)
gm.ENABLE_OBJECT_STATES = True
Expand All @@ -20,28 +21,46 @@ def main(random_selection=False, headless=False, short_exec=False):

# Create the scene config to load -- empty scene + cloth object + hanger + table + rack + robot
cfg = {
"env": {
"external_sensors": [
{
"sensor_type": "VisionSensor",
"name": "helper_camera",
"relative_prim_path": "/external_sensor0",
"modalities": ["rgb"],
"sensor_kwargs": {
"image_height": 1024,
"image_width": 512,
},
"position": [1.7426, -0.0601, 1.4616],
"orientation": [0.4808, 0.3535, 0.4753, 0.6465],
"pose_frame": "parent",
}
],
},
"scene": {
"type": "Scene",
},
"objects": [
{
"type": "DatasetObject",
"name": "pillowcase",
"category": "pillowcase",
"model": "feohwy",
"name": "pants",
"category": "pants",
"model": "tnirgd",
"prim_type": PrimType.CLOTH,
"abilities": {"cloth": {}},
"position": [-0.3, 0, 0.65],
"orientation": [0.0, 0.0, 0.7071, 0.7071],
"position": [100.0, 100.0, 100.0],
"orientation": [-0.5000, -0.5000, 0.5000, 0.5000],
"scale": [0.6, 0.6, 0.6],
},
{
"type": "DatasetObject",
"name": "hanger",
"category": "hanger",
"model": "agrpio",
"prim_type": PrimType.RIGID,
"position": [0.35, 0, 0.68],
"orientation": [0.1379, 0.1379, 0.6935, 0.6935],
"position": [110.0, 100.0, 100.0],
"orientation": [0.0000, 0.0000, 0.7071, 0.7071],
"scale": [2.0, 2.0, 2.0],
},
{
Expand All @@ -50,18 +69,18 @@ def main(random_selection=False, headless=False, short_exec=False):
"category": "breakfast_table",
"model": "skczfi",
"prim_type": PrimType.RIGID,
"position": [0.0, 0.0, 0.4],
"position": [0.0, 0.0, 0.9],
"fixed_base": True,
"scale": [1.0, 1.5, 2.5],
},
{
"type": "DatasetObject",
"name": "rack",
"category": "drying_rack",
"model": "rygebd",
"prim_type": PrimType.RIGID,
"position": [0.8, -0.3, 0.55],
"position": [1.0, -0.3, 0.75],
"orientation": [0.0, 0.0, 0.7071, 0.7071],
"scale": [0.8, 0.8, 0.8],
"fixed_base": True,
},
],
Expand All @@ -71,12 +90,17 @@ def main(random_selection=False, headless=False, short_exec=False):
"obs_modalities": [],
"position": [-0.15, 0.8, 0.0],
"orientation": [0.0, 0.0, -0.7071, 0.7071],
# "controller_config": {
# "arm_left": {
# "name": "InverseKinematicsController",
# "mode": "pose_absolute_ori",
# }
# },
"grasping_mode": "sticky",
"controller_config": {
"arm_left": {
"name": "OperationalSpaceController",
},
"arm_right": {
"name": "OperationalSpaceController",
},
},
"action_type": "continuous",
"action_normalize": True,
}
],
}
Expand All @@ -87,54 +111,56 @@ def main(random_selection=False, headless=False, short_exec=False):
# Grab object references
breakfast_table = env.scene.object_registry("name", "breakfast_table")
hanger = env.scene.object_registry("name", "hanger")
pillowcase = env.scene.object_registry("name", "pillowcase")
pants = env.scene.object_registry("name", "pants")
rack = env.scene.object_registry("name", "rack")
R1 = env.scene.robots[0]

# TODO: fix this when we have a better way to deal with gravity compensation
hanger.root_link.density = 1.0
pants.root_link.mass = 0.01

# Set camera pose
og.sim.viewer_camera.set_position_orientation(
position=th.tensor([0.7729, 1.6733, 1.4305]),
orientation=th.tensor([0.0985, 0.5188, 0.8343, 0.1585]),
position=th.tensor([0.8550, 1.5565, 1.8658]),
orientation=th.tensor([0.1224, 0.5098, 0.8280, 0.1989]),
)

for _ in range(35):
og.sim.step()

# left_eef_target_pos = hanger.get_position_orientation()[0] + th.tensor([0.0, 0.3, 0.0])
# # left_eef_target_ori = T.quat2axisangle(th.tensor([0.0, 0.0, 0.0, 1.0]))
# # left_eef_target_ori = th.tensor([0.0, 0.0, 0.0, 1.0])
# left_eef_target_ori = R1.get_eef_orientation()

# R1.controllers["arm_left"]._goal = {"target_pos": left_eef_target_pos, "target_quat": left_eef_target_ori}
action_generator = KeyboardRobotController(robot=R1)
action_generator.print_keyboard_teleop_info()

# for _ in range(35):
# og.sim.step()
# open the gripper
open_action = action_generator.get_teleop_action()
open_action[R1.controller_action_idx["gripper_left"]] = 1.0

# # Let pillow case settle
# for _ in range(35):
# og.sim.step()
for _ in range(10):
pants.keep_still()
hanger.keep_still()
env.step(open_action)

# target_pos = hanger.get_position_orientation()[0] + th.tensor([0.4, 0.0, 0.1])
# teleport the objects
pants.set_position_orientation([0.1861, 0.1940, 1.2226], [-0.5000, -0.5000, 0.5000, 0.5000])
hanger.set_position_orientation([0.2579, 0.2352, 1.2843], [0.0000, 0.0000, 0.7071, 0.7071])

# pos = pillowcase.root_link.compute_particle_positions()
# # Get the indices for the top 10 percent vertices in the x-axis
# indices = th.argsort(pos, dim=0)[:, 0][-(pos.shape[0] // 10) :]
# start = th.clone(pos[indices])
# close the gripper
close_action = action_generator.get_teleop_action()
close_action[R1.controller_action_idx["gripper_left"]] = -1.0

# start_center = start.mean(dim=0)
# offsets = start - start_center
# end = target_pos.unsqueeze(0) + offsets
for _ in range(10):
env.step(close_action)

# # end = target_pos.repeat(len(indices), 1)
print("Running demo, try to put the hanger onto the rack. ")
print("Press ESC to quit")

# # Number of increments for smooth movement
# increments = 100
# # Move the vertices to the target position
# for ctrl_pts in multi_dim_linspace(start, end, increments):
# pillowcase.root_link.set_particle_positions(ctrl_pts, idxs=indices)
# og.sim.step()
# Loop control until user quits
max_steps = -1 if not short_exec else 100
step = 0

breakpoint()
while step != max_steps:
action = action_generator.get_teleop_action()
# Make sure R1 doesn't release the hanger
action[R1.controller_action_idx["gripper_left"]] = -1.0
env.step(action=action)
step += 1

# Shut down env at the end
og.clear()
Expand Down

0 comments on commit 8814314

Please sign in to comment.