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]