def __init__(self, horizon=500, *args, **kwargs):
options = {}
controller_name = 'OSC_POSE'
options["controller_configs"] = suite.load_controller_config(
default_controller=controller_name)
self.env = GymWrapper(
suite.make(
"NutAssemblyRound", # Nut Assembly task with the round peg
robots="IIWA", # use IIWA robot
**options, # controller options
use_object_obs=True,
use_camera_obs=False, # do not use pixel observations
has_offscreen_renderer=
False, # not needed since not using pixel obs
has_renderer=False, # make sure we can render to the screen
reward_shaping=True, # use dense rewards
reward_scale=1.0,
control_freq=
20, # control should happen fast enough so that simulation looks smooth
horizon=horizon, # number of timesteps for ending episode
))
self.max_episode_steps = horizon
self.action_space = self.env.action_space
self.observation_space = self.env.observation_space
self.reward_type = 'sparse'
self.distance_threshold = 0.065
def test_all_controllers():
for controller_name in controllers.keys():
# Define variables for each controller test
action_dim = controllers[controller_name][0]
num_test_steps = controllers[controller_name][1]
test_value = controllers[controller_name][2]
neutral = np.zeros(action_dim)
# Define controller path to load
controller_config = load_controller_config(default_controller=controller_name)
# Now, create a test env for testing the controller on
env = suite.make(
"Lift",
robots="Sawyer",
has_renderer=args.render, # use on-screen renderer for visual validation only if requested
has_offscreen_renderer=False,
use_camera_obs=False,
horizon=(steps_per_action + steps_per_rest) * num_test_steps,
controller_configs=controller_config,
)
print("Testing controller: {}...".format(controller_name))
env.reset()
# If rendering, set controller to front view to get best angle for viewing robot movements
if args.render:
env.viewer.set_camera(camera_id=0)
# get action range
action_min, action_max = env.action_spec
assert action_min.shape == action_max.shape
assert action_min.shape[0] == action_dim, "Expected {}, got {}".format(action_dim, action_min.shape[0])
# Keep track of done variable to know when to break loop
count = 0
# Loop through controller space
while count < num_test_steps:
action = neutral.copy()
for i in range(steps_per_action):
if controller_name in {"IK_POSE", "OSC_POSE"} and count > 2:
# Set this value to be the angle and set appropriate axis
vec = np.zeros(3)
vec[count - 3] = test_value
action[3:6] = vec
else:
action[count] = test_value
env.step(action)
if args.render:
env.render()
for i in range(steps_per_rest):
env.step(neutral)
if args.render:
env.render()
count += 1
# Shut down this env before starting the next test
env.close()
# Tests passed!
print("All controller tests completed.")
def gather_calibration_measurements():
env = suite.make(
'Ultrasound',
robots='Panda',
controller_configs=load_controller_config(
default_controller='OSC_POSE'),
gripper_types='UltrasoundProbeGripper',
has_renderer=True,
has_offscreen_renderer=False,
use_camera_obs=False,
use_object_obs=False,
control_freq=100,
render_camera=None,
horizon=250,
initialization_noise=None,
)
# Reset the env
env.reset()
robot = env.robots[0]
ee_z_force = np.empty(shape=(env.horizon, 1))
ee_z_pos = np.empty(shape=(env.horizon, 1))
ee_z_vel = np.empty(shape=(env.horizon, 1))
for t in range(env.horizon):
print(t)
if env.done:
break
env.render()
action = [0.2, 0.05, -0.2, 0, 0, 0]
if t > 50:
action = [0, 0, 0, 0, 0, 0]
if t > 75:
action = [0, 0, -0.4, 0, 0, 0]
if t > 200:
action = [0, 0, 0.8, 0, 0, 0]
if t > 225:
action = [0, 0, 0, 0, 0, 0]
observation, reward, done, info = env.step(action)
ee_z_force[t] = transform_ee_frame_axes(
robot.ee_force
)[-1] if robot.gripper_type == 'UltrasoundProbeGripper' else robot.ee_force[
-1]
ee_z_pos[t] = observation['robot0_eef_pos'][-1]
ee_z_vel[t] = observation['gripper_velp'][-1]
np.savetxt('data/calibration_z_force.csv', ee_z_force, delimiter=",")
np.savetxt('data/calibration_z_pos.csv', ee_z_pos, delimiter=",")
np.savetxt('data/calibration_z_vel.csv', ee_z_vel, delimiter=",")
# close window
env.close()
def make(has_renderer=False,
init_robot_pose=(-1.37550484e-02, 5.21560077e-03, 8.78072546e-02),
renderer=None,
horizon=100):
controller_conf = load_controller_config(
default_controller="LOCOMOTION_JOINT_TORQUE")
os.makedirs(log_dir, exist_ok=True)
rbs_env = suite.make(env_name="Walk",
robots="Laikago",
controller_configs=controller_conf,
has_renderer=has_renderer,
render_camera=renderer,
has_offscreen_renderer=False,
use_camera_obs=False,
control_freq=10,
init_robot_pose=init_robot_pose,
init_robot_ori=(0, 0, 0),
reward_shaping=True,
horizon=horizon)
env = GymWrapper(rbs_env,
keys=[
"robot0_chassis_pos", "robot0_chassis_vel",
"robot0_joint_pos", "robot0_joint_vel"
])
if not has_renderer:
env = Monitor(env, log_dir)
print("box:")
print(env.action_space)
print("shape -1")
print(env.action_space.shape[-1])
print("shape")
print(env.action_space.shape)
print("high")
print(env.action_space.high)
print("low")
print(env.action_space.low)
print("box:")
print(env.observation_space)
print("shape -1")
print(env.observation_space.shape[-1])
print("shape")
print(env.observation_space.shape)
print("high")
print(env.observation_space.high)
print("low")
print(env.observation_space.low)
return env
def make_env():
controller = load_controller_config(default_controller="OSC_POSE")
env = GymWrapper(
suite.make(
"Door",
robots="Panda", # use Sawyer robot
use_camera_obs=False, # do not use pixel observations
has_offscreen_renderer=False, # not needed since not using pixel obs
has_renderer=False, # make sure we can render to the screen
reward_shaping=True, # use dense rewards
reward_scale=1.0, # scale max 1 per timestep
control_freq=
20, # control should happen fast enough so that simulation looks smooth
horizon=500,
ignore_done=True,
hard_reset=False,
controller_configs=controller))
return env
def _load_model(self):
# Load the desired controller's default config as a dict
controller_config = load_controller_config(
default_controller="JOINT_VELOCITY")
controller_config["output_max"] = 1.0
controller_config["output_min"] = -1.0
robot_noise = {"magnitude": [0.05] * 7, "type": "gaussian"}
self.robot = SingleArm(
robot_type="IIWA",
idn=0,
controller_config=controller_config,
initial_qpos=[0.0, 0.7, 0.0, -1.4, 0.0, -0.56, 0.0],
initialization_noise=robot_noise,
gripper_type="PaddleGripper",
gripper_visualization=True,
control_freq=self.control_freq)
self.robot.load_model()
self.robot.robot_model.set_base_xpos([0, 0, 0])
self.arena = EmptyArena()
self.arena.set_origin([0.8, 0, 0])
self.pingpong = BallObject(name="pingpong",
size=[0.02],
rgba=[0.8, 0.8, 0, 1],
solref=[0.1, 0.03],
solimp=[0, 0, 1],
density=100)
pingpong_model = self.pingpong.get_collision()
pingpong_model.append(
new_joint(name="pingpong_free_joint", type="free"))
pingpong_model.set("pos", "0.8 0 2.0")
# merge into one
self.model = MujocoWorldBase()
self.model.merge(self.robot.robot_model)
self.model.merge(self.arena)
self.model.worldbody.append(pingpong_model)
print_command("Keys", "Command")
print_command(
"1-N",
"Active Joint being tuned (N=number of joints for the active arm)")
print_command("t", "Toggle between robot arms in the environment")
print_command("r", "Reset active arm joints to all 0s")
print_command("up/down", "incr/decrement the active joint angle")
print_command("right/left",
"incr/decrement the delta joint angle per up/down keypress")
print("")
# Setup printing options for numbers
np.set_printoptions(formatter={'float': lambda x: "{0:0.3f}".format(x)})
# Define the controller
controller_config = robosuite.load_controller_config(
default_controller="JOINT_POSITION")
# make the environment
roboenv = robosuite.make(args.env,
robots=args.robots,
has_renderer=True,
has_offscreen_renderer=False,
ignore_done=True,
use_camera_obs=False,
control_freq=20,
render_camera=None,
controller_configs=controller_config,
initialization_noise=None)
roboenv.reset()
# register callbacks to handle key presses in the viewer
parser.add_argument("--device", type=str, default="keyboard")
parser.add_argument("--pos-sensitivity", type=float, default=1.0, help="How much to scale position user inputs")
parser.add_argument("--rot-sensitivity", type=float, default=1.0, help="How much to scale rotation user inputs")
args = parser.parse_args()
# Import controller config for EE IK or OSC (pos/ori)
if args.controller == "ik":
controller_name = "IK_POSE"
elif args.controller == "osc":
controller_name = "OSC_POSE"
else:
print("Error: Unsupported controller specified. Must be either 'ik' or 'osc'!")
raise ValueError
# Get controller config
controller_config = load_controller_config(default_controller=controller_name)
# Create argument configuration
config = {
"env_name": args.environment,
"robots": args.robots,
"controller_configs": controller_config,
}
# Check if we're using a multi-armed environment and use env_configuration argument if so
if "TwoArm" in args.environment:
config["env_configuration"] = args.config
else:
args.config = None
# Create environment
parser.add_argument("--environment", type=str, default="Lift")
parser.add_argument("--robots", nargs="+", type=str, default="Panda", help="Which robot(s) to use in the env")
parser.add_argument("--config", type=str, default="single-arm-opposed",
help="Specified environment configuration if necessary")
parser.add_argument("--arm", type=str, default="right", help="Which arm to control (eg bimanual) 'right' or 'left'")
parser.add_argument("--camera", type=str, default="agentview", help="Which camera to use for collecting demos")
parser.add_argument("--controller", type=str, default="OSC_POSE",
help="Choice of controller. Can be 'IK_POSE' or 'OSC_POSE'")
parser.add_argument("--device", type=str, default="keyboard")
parser.add_argument("--pos-sensitivity", type=float, default=1.0, help="How much to scale position user inputs")
parser.add_argument("--rot-sensitivity", type=float, default=1.0, help="How much to scale rotation user inputs")
args = parser.parse_args()
# Get controller config
controller_config = load_controller_config(default_controller=args.controller)
# Create argument configuration
config = {
"env_name": args.environment,
"robots": args.robots,
"controller_configs": controller_config,
}
# Check if we're using a multi-armed environment and use env_configuration argument if so
if "TwoArm" in args.environment:
config["env_configuration"] = args.config
# Create environment
env = suite.make(
**config,
num_resnet_layers = 50
sequence_length = args.sequence_length
feature_layer_nums = (9, )
# Define placement initializer for object
rotation_axis = 'y' if args.obj_name == 'hammer' else 'z'
placement_initializer = UniformRandomSampler(
x_range=[-0.35, 0.35],
y_range=[-0.35, 0.35],
ensure_object_boundary_in_range=False,
rotation=None,
rotation_axis=rotation_axis,
) if args.use_placement_initializer else None
# Define robosuite model
controller_config = suite.load_controller_config(
default_controller=args.controller)
env = suite.make(
args.env,
robots=args.robots,
#has_renderer=True,
#render_camera=camera_name,
has_offscreen_renderer=True,
camera_depths=args.use_depth,
use_camera_obs=True,
horizon=horizon,
camera_names=camera_name,
controller_configs=controller_config,
initialization_noise=initialization_noise,
placement_initializer=placement_initializer)
if __name__ == '__main__':
register_gripper(UltrasoundProbeGripper)
env = GymWrapper(suite.make(env_id, **options))
env = Monitor(env)
env.seed(seed + rank)
return env
set_random_seed(seed)
return _init
if __name__ == '__main__':
register_env(FetchPush)
# Environment specifications
env_id = 'FetchPush'
options = {}
options['robots'] = 'UR5e'
options['controller_configs'] = load_controller_config(default_controller='OSC_POSE')
options['gripper_types'] = 'UltrasoundProbeGripper'
options['has_renderer'] = False
options['has_offscreen_renderer'] = False
options['use_camera_obs'] = False
options['use_object_obs'] = True
options['control_freq'] = 50
options['render_camera'] = None
options['horizon'] = 1000
options['reward_shaping'] = True
# Settings
training = False
training_timesteps = 2e6
num_cpu = 4
tb_log_folder = 'ppo_fetchpush_tensorboard'
for i in range(2):
print("Please choose Robot {}...\n".format(i))
options["robots"].append(choose_robots(exclude_bimanual=True))
# Else, we simply choose a single (single-armed) robot to instantiate in the environment
else:
options["robots"] = choose_robots(exclude_bimanual=True)
# Hacky way to grab joint dimension for now
joint_dim = 6 if options["robots"] == "UR5e" else 7
# Choose controller
controller_name = choose_controller()
# Load the desired controller
options["controller_configs"] = suite.load_controller_config(
default_controller=controller_name)
# Define the pre-defined controller actions to use (action_dim, num_test_steps, test_value)
controller_settings = {
"OSC_POSE": [6, 6, 0.1],
"OSC_POSITION": [3, 3, 0.1],
"IK_POSE": [6, 6, 0.01],
"JOINT_POSITION": [joint_dim, joint_dim, 0.2],
"JOINT_VELOCITY": [joint_dim, joint_dim, -0.1],
"JOINT_TORQUE": [joint_dim, joint_dim, 0.25],
}
# Define variables for each controller test
action_dim = controller_settings[controller_name][0]
num_test_steps = controller_settings[controller_name][1]
test_value = controller_settings[controller_name][2]
import torch
from robosuite import load_controller_config
from robosuite.utils.placement_samplers import UniformRandomSampler
import rlkit.util.hyperparameter as hyp
from rlkit.launchers.launcher_util import run_experiment
def experiment(variant):
from a2c_ppo_acktr.main import experiment
experiment(variant)
if __name__ == "__main__":
config = load_controller_config(default_controller="OSC_POSE")
parser = argparse.ArgumentParser()
parser.add_argument("--exp_prefix", type=str, default="test")
parser.add_argument("--num_seeds", type=int, default=1)
parser.add_argument("--mode", type=str, default="local")
parser.add_argument("--debug", action="store_true", default=False)
args = parser.parse_args()
if args.debug:
exp_prefix = "test" + args.exp_prefix
else:
exp_prefix = args.exp_prefix
variant = dict(
algorithm_kwargs=dict(
entropy_coef=0.01,
value_loss_coef=0.5,
lr=3e-4,
def __init__(self, *args, **kwargs):
options = {}
# Absolute pose control
controller_name = 'OSC_POSE'
options["controller_configs"] = suite.load_controller_config(
default_controller=controller_name)
self.cameraName = "frontview"
skip_frame = 2
self.peg_env = suite.make(
"TwoArmPegInHole",
robots=["IIWA", "IIWA"],
**options,
has_renderer=False,
ignore_done=True,
use_camera_obs=True,
use_object_obs=True,
camera_names=self.cameraName,
camera_heights=512,
camera_widths=512,
)
# Tolerances on position and rotation of peg relative to hole
posTol = 0.005
rotTol = 0.05
observation = self.peg_env.reset()
# observation["peg_to_hole"] is the vector FROM the hole TO the peg
peg_pos0 = observation["hole_pos"] + observation["peg_to_hole"]
self.peg_pos0 = peg_pos0
self.hole_pos0 = observation["hole_pos"]
# Positions of robots 0 and 1 rel peg and hole, in peg and hole frames. Constant forever.
pRob0RelPeg = np.matmul(
T.quat2mat(T.quat_inverse(observation["peg_quat"])),
observation["robot0_eef_pos"] - (peg_pos0))
pRob1RelHole = np.matmul(
T.quat2mat(T.quat_inverse(observation["hole_quat"])),
observation["robot1_eef_pos"] - observation["hole_pos"])
qRob0RelPeg = T.quat_multiply(
T.quat_inverse(observation["robot0_eef_quat"]),
observation["peg_quat"])
qRob1RelHole = T.quat_multiply(
T.quat_inverse(observation["robot1_eef_quat"]),
observation["hole_quat"])
# Store geometric constants
model = self.peg_env.model.get_model()
pegDim = model.geom_size[15]
rPeg = pegDim[0]
lPeg = pegDim[2]
distSlack = 2 * lPeg
# Set up optimization problem.
# Define 3 keyframes: peg higher than hole, peg centered above hole with hole facing up, and peg in hole.
# One constraint for each keyframe, and one constraint for unit quaternions
nonlinear_constraint_1 = NonlinearConstraint(self.cons_1,
distSlack,
np.inf,
jac='2-point',
hess=BFGS())
nonlinear_constraint_2 = NonlinearConstraint(
self.cons_2,
np.array([-posTol, -posTol, distSlack, -rotTol]),
np.array([posTol, posTol, np.inf, rotTol]),
jac='2-point',
hess=BFGS())
nonlinear_constraint_3 = NonlinearConstraint(
self.cons_3,
np.array([-posTol, -posTol, distSlack]),
np.array([posTol, posTol, np.inf]),
jac='2-point',
hess=BFGS())
nonlinear_constraint_4 = NonlinearConstraint(self.cons_unit_quat,
np.array([1, 1, 1, 1]),
np.array([1, 1, 1, 1]),
jac='2-point',
hess=BFGS())
# Initial guess for optimizer
x0 = np.tile(
np.hstack((peg_pos0, observation["hole_pos"],
observation["peg_quat"], observation["hole_quat"])), 3)
# Cut out quat from last chunk
x0 = x0[0:34]
# Solve optimization problem
res = minimize(self.traj_obj,
x0,
method='trust-constr',
jac='2-point',
hess=BFGS(),
constraints=[
nonlinear_constraint_1, nonlinear_constraint_2,
nonlinear_constraint_3, nonlinear_constraint_4
],
options={'verbose': 1})
# 'xtol': 1e-6,
x = res.x
# Extract optimization results
# x = [p_peg_1, p_hole_1, q_peg_1, q_hole_1, p_peg_2, ... q_peg_3, q_hole_3]
ind_offset_1 = 14
ind_offset_2 = 28
p_peg_1 = x[0:3]
p_hole_1 = x[3:6]
p_peg_2 = x[ind_offset_1 + 0:ind_offset_1 + 3]
p_hole_2 = x[ind_offset_1 + 3:ind_offset_1 + 6]
p_peg_3 = x[ind_offset_2 + 0:ind_offset_2 + 3]
p_hole_3 = x[ind_offset_2 + 3:ind_offset_2 + 6]
q_peg_1 = x[6:10]
q_hole_1 = x[10:14]
q_peg_2 = x[ind_offset_1 + 6:ind_offset_1 + 10]
q_hole_2 = x[ind_offset_1 + 10:ind_offset_1 + 14]
# Use the same orientations as in pose 2
q_peg_3 = q_peg_2
q_hole_3 = q_hole_2
# Robot rel world = peg rel world + robot rel peg
# Robot rel peg in world frame = (q world frame rel peg frame) * (robot rel peg in peg frame)
q_rob0_1 = T.quat_inverse(
T.quat_multiply(qRob0RelPeg, T.quat_inverse(q_peg_1)))
q_rob1_1 = T.quat_inverse(
T.quat_multiply(qRob1RelHole, T.quat_inverse(q_hole_1)))
q_rob0_2 = T.quat_inverse(
T.quat_multiply(qRob0RelPeg, T.quat_inverse(q_peg_2)))
q_rob1_2 = T.quat_inverse(
T.quat_multiply(qRob1RelHole, T.quat_inverse(q_hole_2)))
q_rob0_3 = T.quat_inverse(
T.quat_multiply(qRob0RelPeg, T.quat_inverse(q_peg_3)))
q_rob1_3 = T.quat_inverse(
T.quat_multiply(qRob1RelHole, T.quat_inverse(q_hole_3)))
self.p_rob0_1 = p_peg_1 + np.matmul(T.quat2mat(q_peg_1), pRob0RelPeg)
self.p_rob1_1 = p_hole_1 + np.matmul(T.quat2mat(q_hole_1),
pRob1RelHole)
self.p_rob0_2 = p_peg_2 + np.matmul(T.quat2mat(q_peg_2), pRob0RelPeg)
self.p_rob1_2 = p_hole_2 + np.matmul(T.quat2mat(q_hole_2),
pRob1RelHole)
self.p_rob0_3 = p_peg_3 + np.matmul(T.quat2mat(q_peg_3), pRob0RelPeg)
self.p_rob1_3 = p_hole_3 + np.matmul(T.quat2mat(q_hole_3),
pRob1RelHole)
self.axang_rob0_1 = T.quat2axisangle(q_rob0_1)
self.axang_rob1_1 = T.quat2axisangle(q_rob1_1)
self.axang_rob0_2 = T.quat2axisangle(q_rob0_2)
self.axang_rob1_2 = T.quat2axisangle(q_rob1_2)
self.axang_rob0_3 = T.quat2axisangle(q_rob0_3)
self.axang_rob1_3 = T.quat2axisangle(q_rob1_3)
self.max_episode_steps = 200
# Gains for rotation and position error compensation rates
self.kpp = 4
self.kpr = 0.1
# Initial pose Information
self.rob0quat0 = observation["robot0_eef_quat"]
self.rob1quat0 = observation["robot1_eef_quat"]
self.rob0pos0 = observation["robot0_eef_pos"]
self.rob1pos0 = observation["robot1_eef_pos"]
# Choose environment and add it to options
options["env_name"] = "ReachBaby"
options["robots"] = robot
# Hacky way to grab joint dimension for now
joint_dim = 6 if options["robots"] == "UR5e" else 7
controller_name = 'JOINT_POSITION'
controller_file = "%s_%s_%shz.json" % (
robot.lower(), controller_name.lower(), control_freq)
controller_fpath = os.path.join(
os.path.split(suite.__file__)[0], 'controllers', 'config',
controller_file)
# Load the desired controller
options["controller_configs"] = suite.load_controller_config(
custom_fpath=controller_fpath)
# Define the pre-defined controller actions to use (action_dim, num_test_steps, test_value)
controller_settings = {
"OSC_POSE": [6, 6, 0.1],
"OSC_POSITION": [3, 3, 0.1],
"IK_POSE": [6, 6, 0.01],
"JOINT_POSITION": [joint_dim, joint_dim, 0.2],
"JOINT_VELOCITY": [joint_dim, joint_dim, -0.1],
"JOINT_TORQUE": [joint_dim, joint_dim, 0.25]
}
# Define variables for each controller test
action_dim = controller_settings[controller_name][0]
num_test_steps = controller_settings[controller_name][1]
test_value = controller_settings[controller_name][2]
