reward_shaping=True,
control_freq=20,
hard_reset=False,
)
# Wrap this environment in a visualization wrapper
env = VisualizationWrapper(env, indicator_configs=None)
# Setup printing options for numbers
np.set_printoptions(formatter={"float": lambda x: "{0:0.3f}".format(x)})
# initialize device
if args.device == "keyboard":
from robosuite.devices import Keyboard
device = Keyboard(pos_sensitivity=args.pos_sensitivity, rot_sensitivity=args.rot_sensitivity)
env.viewer.add_keypress_callback("any", device.on_press)
env.viewer.add_keyup_callback("any", device.on_release)
env.viewer.add_keyrepeat_callback("any", device.on_press)
elif args.device == "spacemouse":
from robosuite.devices import SpaceMouse
device = SpaceMouse(pos_sensitivity=args.pos_sensitivity, rot_sensitivity=args.rot_sensitivity)
else:
raise Exception("Invalid device choice: choose either 'keyboard' or 'spacemouse'.")
while True:
# Reset the environment
obs = env.reset()
# Setup rendering
)
# enable controlling the end effector directly instead of using joint velocities
pre_env = GymWrapper(pre_env)
env = IKWrapper(env) # note cannot disable this or things go wack
# wrap the environment with data collection wrapper
tmp_directory = "~/Robotics/{}".format(str(time.time()).replace(
".", "_")) # Change from temp to Robotics folder
env = DataCollectionWrapperBaseline(env, tmp_directory)
# initialize device
if args.device == "keyboard":
from robosuite.devices import Keyboard
device = Keyboard()
env.viewer.add_keypress_callback("any", device.on_press)
env.viewer.add_keyup_callback("any", device.on_release)
env.viewer.add_keyrepeat_callback("any", device.on_press)
elif args.device == "spacemouse":
from robosuite.devices import SpaceMouse
device = SpaceMouse()
else:
raise Exception(
"Invalid device choice: choose either 'keyboard' or 'spacemouse'.")
# make a new timestamped directory
t1, t2 = str(time.time()).split(".")
new_dir = os.path.join(args.directory, "{}_{}".format(t1, t2))
os.makedirs(new_dir)
# Wrap this with visualization wrapper
env = VisualizationWrapper(env)
# Grab reference to controller config and convert it to json-encoded string
env_info = json.dumps(config)
# wrap the environment with data collection wrapper
tmp_directory = "/tmp/{}".format(str(time.time()).replace(".", "_"))
env = DataCollectionWrapper(env, tmp_directory)
# initialize device
if args.device == "keyboard":
from robosuite.devices import Keyboard
device = Keyboard(pos_sensitivity=args.pos_sensitivity, rot_sensitivity=args.rot_sensitivity)
env.viewer.add_keypress_callback("any", device.on_press)
env.viewer.add_keyup_callback("any", device.on_release)
env.viewer.add_keyrepeat_callback("any", device.on_press)
elif args.device == "spacemouse":
from robosuite.devices import SpaceMouse
device = SpaceMouse(pos_sensitivity=args.pos_sensitivity, rot_sensitivity=args.rot_sensitivity)
else:
raise Exception(
"Invalid device choice: choose either 'keyboard' or 'spacemouse'."
)
# make a new timestamped directory
t1, t2 = str(time.time()).split(".")
new_dir = os.path.join(args.directory, "{}_{}".format(t1, t2))
def main_osc(controller_kp=[1500, 1500, 50, 150, 150, 150],
controller_zeta=[1.5, 1.5, 1, 10, 10, 10],
perception_error=0.0,
offscreen=False):
# Task configuration
# option:
# board : Hole 12mm, Hole 9mm
# peg : 16mm. 12mm, 9mm
# USB : USB-C
task_config = {'board': 'Square_hole_16mm', 'peg': 'cylinder_16mm'}
# IMP_OSC is a custom controller written by us.
# find out the source code at https://github.com/garlicbutter/robosuite
# Theory based on the paper by Valency: https://doi.org/10.1115/1.1590685.
controller_config = {
"type": "OSC_POSE",
"input_max": 1,
"input_min": -1,
"output_max": [0.05, 0.05, 0.05, 0.5, 0.5, 0.5],
"output_min": [-0.05, -0.05, -0.05, -0.5, -0.5, -0.5],
"kp": controller_kp,
"damping_ratio": controller_zeta,
"impedance_mode": "fixed",
"kp_limits": [[0, 300], [0, 300], [0, 300], [0, 300], [0, 300],
[0, 300]],
"damping_ratio_limits": [0, 10],
"position_limits": None,
"orientation_limits": None,
"uncouple_pos_ori": True,
"control_delta": True,
"interpolation": None,
"ramp_ratio": 0.2
}
if offscreen:
# create on-screen environment instance
env = MyEnvOffScreen(robots="UR5e",
task_configs=task_config,
controller_configs=controller_config,
gripper_types='Robotiq85Gripper',
ignore_done=True)
else:
# create on-screen environment instance
env = MyEnv(robots="UR5e",
task_configs=task_config,
controller_configs=controller_config,
gripper_types='Robotiq85Gripper',
has_renderer=True,
has_offscreen_renderer=False,
use_object_obs=True,
use_camera_obs=False,
ignore_done=True,
render_camera=None)
# create motion planning class
motion_planning = motion_planning_osc.Policy_action(env.control_timestep,
P=3,
I=0.1)
# manual control via keyboard
manual_control = False
if manual_control:
from robosuite.devices import Keyboard
from robosuite.utils.input_utils import input2action
device = Keyboard(pos_sensitivity=0.05, rot_sensitivity=1.0)
env.viewer.add_keypress_callback("any", device.on_press)
env.viewer.add_keyup_callback("any", device.on_release)
env.viewer.add_keyrepeat_callback("any", device.on_press)
# Initial recorder
eeff_record = np.empty([1, 3]) # end effector force record
robot_torque_record = np.empty([1, 6]) # Actuation torque record
eeft_record = np.empty([1, 3]) # end effector torque record
eefd_record = np.empty([1, 3]) # end effector displacement record
eefd_desire_record = np.empty(
[1, 3]) # desired end effector displacement record
eefd_desire = np.empty(
[1, 3]) # to calculate desired displacement at each moment
t_record = np.empty(1) # time record
# Initial action
action = np.zeros(env.robots[0].dof)
action_status = None
# simulate termination condition
done = False
# simulation
# for perception_error
import random
random_radiant = 2 * np.pi * random.random()
perception_err_x = np.cos(random_radiant) * perception_error
perception_err_y = np.sin(random_radiant) * perception_error
while not done:
obs, reward, done, _ = env.step(
action) # take action in the environment
if manual_control:
action, grasp = input2action(
device=device,
robot=env.robots[0],
)
else:
obs['plate_pos'][0] += perception_err_x
obs['plate_pos'][1] += perception_err_y
# update observation to motion planning11
motion_planning.update_obs(obs)
# decide which action to take for next simulation
action, action_status = motion_planning.get_policy_action()
if not offscreen:
env.render()
eeff_record = np.append(eeff_record, [env.robots[0].ee_force], axis=0)
eeft_record = np.append(eeft_record, [env.robots[0].ee_torque], axis=0)
eefd_record = np.append(eefd_record, [obs['robot0_eef_pos']], axis=0)
eefd_desire = np.add(eefd_desire, action[0:3])
eefd_desire_record = np.append(eefd_desire_record, eefd_desire, axis=0)
robot_torque_record = np.append(robot_torque_record,
[env.robots[0].torques],
axis=0)
t_record = np.append(t_record, np.array([env.cur_time]), axis=0)
if action_status['done']:
break
if t_record[-1] > 30: # failure case
eeff_record = 0
eeft_record = 0
eefd_record = 0
t_record = 0
break
return eeff_record, eeft_record, eefd_record, t_record, robot_torque_record
has_renderer=True,
ignore_done=True,
use_camera_obs=False,
gripper_visualization=True,
reward_shaping=True,
control_freq=100,
)
# enable controlling the end effector directly instead of using joint velocities
env = IKWrapper(env)
# initialize device
if args.device == "keyboard":
from robosuite.devices import Keyboard
device = Keyboard()
env.viewer.add_keypress_callback("any", device.on_press)
env.viewer.add_keyup_callback("any", device.on_release)
env.viewer.add_keyrepeat_callback("any", device.on_press)
elif args.device == "spacemouse":
from robosuite.devices import SpaceMouse
device = SpaceMouse()
else:
raise Exception(
"Invalid device choice: choose either 'keyboard' or 'spacemouse'.")
while True:
obs = env.reset()
env.viewer.set_camera(camera_id=2)
env.render()
"attrs": attributes[:2],
"mods": lambda: proprio_modifiers[:2] if corruption_mode else [None, None]
}
obs_settings[proprio_ground_truth_obs_name] = {
"attrs": [attributes[1]],
"mods": lambda: [lambda: curr_proprio_delay] if corruption_mode else [None]
}
# Setup printing options for numbers
np.set_printoptions(precision=3, suppress=True, floatmode='fixed')
# initialize device
if args.device == "keyboard":
from robosuite.devices import Keyboard
device = Keyboard(pos_sensitivity=args.pos_sensitivity, rot_sensitivity=args.rot_sensitivity)
# Define wrapper method for keyboard callback that only uses the key
pygame.key.set_repeat(20)
on_press = lambda key: device.on_press(None, ord(chr(key).capitalize()), None, None, None)
on_release = lambda key: device.on_release(None, ord(chr(key).capitalize()), None, None, None)
elif args.device == "spacemouse":
from robosuite.devices import SpaceMouse
device = SpaceMouse(pos_sensitivity=args.pos_sensitivity, rot_sensitivity=args.rot_sensitivity)
else:
raise Exception(
"Invalid device choice: choose either 'keyboard' or 'spacemouse'."
)
while True:
control_freq=100,
gripper_visualization=True,
)
# enable controlling the end effector directly instead of using joint velocities
env = IKWrapper(env)
# wrap the environment with data collection wrapper
tmp_directory = "/tmp/{}".format(str(time.time()).replace(".", "_"))
env = DataCollectionWrapper(env, tmp_directory)
# initialize devices
if args.device1 == "keyboard":
from robosuite.devices import Keyboard
device1 = Keyboard()
env.viewer.add_keypress_callback("any", device1.on_press)
env.viewer.add_keyup_callback("any", device1.on_release)
env.viewer.add_keyrepeat_callback("any", device1.on_press)
elif args.device1 == "spacemouse":
from robosuite.devices import SpaceMouse
device1 = SpaceMouse()
else:
raise Exception(
"Invalid device choice: choose either 'keyboard' or 'spacemouse'.")
if args.device2 == "keyboard":
from robosuite.devices import Keyboard
device2 = Keyboard()
def teleoperate(self, demons_config):
env = self.get_env()
# Need to use inverse-kinematics controller to set position using device
env = IKWrapper(env)
if demons_config.device == "keyboard":
from robosuite.devices import Keyboard
device = Keyboard()
env.viewer.add_keypress_callback("any", device.on_press)
env.viewer.add_keyup_callback("any", device.on_release)
env.viewer.add_keyrepeat_callback("any", device.on_press)
elif demons_config.device == "spacemouse":
from robosuite.devices import SpaceMouse
device = SpaceMouse()
for run in range(demons_config.n_runs):
obs = env.reset()
env.set_robot_joint_positions([0, -1.18, 0.00, 2.18, 0.00, 0.57, 1.5708])
# rotate the gripper so we can see it easily - NOTE : REMOVE MAYBE
env.viewer.set_camera(camera_id=2)
env.render()
device.start_control()
reset = False
task_completion_hold_count = -1
step = 0
tr_vobvs, tr_dof, tr_actions = [], [], []
while not reset:
if int(step % demons_config.collect_freq) == 0:
tr_vobvs.append(np.array(obs[self.vis_obv_key]))
tr_dof.append(np.array(obs[self.dof_obv_key].flatten()))
device_state = device.get_controller_state()
dpos, rotation, grasp, reset = (
device_state["dpos"],
device_state["rotation"],
device_state["grasp"],
device_state["reset"],
)
current = env._right_hand_orn
drotation = current.T.dot(rotation)
dquat = T.mat2quat(drotation)
grasp = grasp - 1.
ik_action = np.concatenate([dpos, dquat, [grasp]])
obs, _, done, _ = env.step(ik_action)
env.render()
joint_velocities = np.array(env.controller.commanded_joint_velocities)
if env.env.mujoco_robot.name == "sawyer":
gripper_actuation = np.array(ik_action[7:])
elif env.env.mujoco_robot.name == "baxter":
gripper_actuation = np.array(ik_action[14:])
# NOTE: Action for the normal environment (not inverse kinematic)
action = np.concatenate([joint_velocities, gripper_actuation], axis=0)
if int(step % demons_config.collect_freq) == 0:
tr_actions.append(action)
if (int(step % demons_config.flush_freq) == 0) or (demons_config.break_traj_success and task_completion_hold_count == 0):
print("Storing Trajectory")
trajectory = {self.vis_obv_key : np.array(tr_vobvs), self.dof_obv_key : np.array(tr_dof), 'action' : np.array(tr_actions)}
store_trajectoy(trajectory, 'teleop')
trajectory, tr_vobvs, tr_dof, tr_actions = {}, [], [], []
if demons_config.break_traj_success and env._check_success():
if task_completion_hold_count > 0:
task_completion_hold_count -= 1 # latched state, decrement count
else:
task_completion_hold_count = 10 # reset count on first success timestep
else:
task_completion_hold_count = -1
step += 1
env.close()
ignore_done=True,
use_camera_obs=False,
gripper_visualizations=True,
reward_shaping=True,
control_freq=20,
hard_reset=False,
)
# Setup printing options for numbers
np.set_printoptions(formatter={'float': lambda x: "{0:0.3f}".format(x)})
# initialize device
if args.device == "keyboard":
from robosuite.devices import Keyboard
device = Keyboard(pos_sensitivity=args.pos_sensitivity,
rot_sensitivity=args.rot_sensitivity)
env.viewer.add_keypress_callback("any", device.on_press)
env.viewer.add_keyup_callback("any", device.on_release)
env.viewer.add_keyrepeat_callback("any", device.on_press)
elif args.device == "spacemouse":
from robosuite.devices import SpaceMouse
device = SpaceMouse(pos_sensitivity=args.pos_sensitivity,
rot_sensitivity=args.rot_sensitivity)
elif args.device == "vivetracker":
from robosuite.devices import HTCViveTracker
device = HTCViveTracker()
else:
raise Exception(
"Invalid device choice: choose either 'keyboard' or 'spacemouse' or 'vivetracker'."
gripper_types='Robotiq85Gripper',
has_renderer=True,
has_offscreen_renderer=False,
use_camera_obs=False,
render_camera=None,
ignore_done=True)
# create motion planning class
motion_planning = motion_planning_imp.Policy_action(env.control_timestep,
P=1,
I=0.1)
# manual control via keyboard
manual_control = False
if manual_control:
from robosuite.devices import Keyboard
from robosuite.utils.input_utils import input2action
device = Keyboard(pos_sensitivity=0.05, rot_sensitivity=1.0)
env.viewer.add_keypress_callback("any", device.on_press)
env.viewer.add_keyup_callback("any", device.on_release)
env.viewer.add_keyrepeat_callback("any", device.on_press)
# Initial action
action = np.zeros(env.robots[0].dof)
# simulate termination condition
done = False
# simulation
while not done:
obs, reward, done, _ = env.step(
action) # take action in the environment
F_int = env.robots[0].ee_force
if manual_control:
action, grasp = input2action(
device=device,