diff --git a/experiments/vrep_example/hyperparams.py b/experiments/vrep_example/hyperparams.py new file mode 100644 index 000000000..8040ebf72 --- /dev/null +++ b/experiments/vrep_example/hyperparams.py @@ -0,0 +1,141 @@ +""" Hyperparameters for VREP peg insertion trajectory optimization.""" + +from __future__ import division + +from datetime import datetime +import math +import os.path +import numpy as np + +from gps import __file__ as gps_filepath +from gps.agent.vrep.agent_vrep import AgentVREP +from gps.algorithm.algorithm_traj_opt import AlgorithmTrajOpt +from gps.algorithm.cost.cost_fk import CostFK +from gps.algorithm.cost.cost_action import CostAction +from gps.algorithm.cost.cost_sum import CostSum +from gps.algorithm.dynamics.dynamics_lr_prior import DynamicsLRPrior +from gps.algorithm.dynamics.dynamics_prior_gmm import DynamicsPriorGMM +from gps.algorithm.traj_opt.traj_opt_lqr_python import TrajOptLQRPython +from gps.algorithm.policy.lin_gauss_init import init_lqr +from gps.proto.gps_pb2 import JOINT_ANGLES, JOINT_VELOCITIES, \ + END_EFFECTOR_POINTS, END_EFFECTOR_POINT_VELOCITIES, ACTION +from gps.gui.config import generate_experiment_info + +SENSOR_DIMS = { + JOINT_ANGLES: 7, + JOINT_VELOCITIES: 7, + END_EFFECTOR_POINTS: 6, + END_EFFECTOR_POINT_VELOCITIES: 6, + ACTION: 7, +} + +PR2_GAINS = np.array([3.09, 1.08, 0.393, 0.674, 0.111, 0.152, 0.098]) + +BASE_DIR = '/'.join(str.split(gps_filepath, '/')[:-2]) +EXP_DIR = BASE_DIR + '/../experiments/vrep_example/' + + +common = { + 'experiment_name': 'my_experiment' + '_' + \ + datetime.strftime(datetime.now(), '%m-%d-%y_%H-%M'), + 'experiment_dir': EXP_DIR, + 'data_files_dir': EXP_DIR + 'data_files/', + 'target_filename': EXP_DIR + 'target.npz', + 'log_filename': EXP_DIR + 'log.txt', + 'conditions': 1, +} + +if not os.path.exists(common['data_files_dir']): + os.makedirs(common['data_files_dir']) + +x0_base = np.concatenate([np.array([-2.703e+01, +3.144e+00, -3.169e+01, + -2.318e+01, -7.945e+01, -2.970e+01, + +6.623e+01]), + np.zeros(7)]) + +# Convert degrees to radians. +x0_base *= (math.pi / 180.0) + +agent = { + 'type': AgentVREP, + 'filename': './vrep_models/peg_insertion_torque.ttt', + 'x0': x0_base, + 'dt': 0.05, + 'substeps': 1, + 'conditions': common['conditions'], + 'pos_body_idx': np.array([1]), + 'pos_body_offset': np.array([0, 0.2, 0]), + 'T': 100, + 'sensor_dims': SENSOR_DIMS, + 'state_include': [JOINT_ANGLES, JOINT_VELOCITIES, END_EFFECTOR_POINTS, + END_EFFECTOR_POINT_VELOCITIES], + 'obs_include': [], + 'camera_pos': np.array([0., 0., 2., 0., 0.2, 0.5]), +} + +algorithm = { + 'type': AlgorithmTrajOpt, + 'conditions': common['conditions'], + 'iterations': 10, +} + +algorithm['init_traj_distr'] = { + 'type': init_lqr, + 'init_gains': 1.0 / PR2_GAINS, + 'init_acc': np.zeros(SENSOR_DIMS[ACTION]), + 'init_var': 1.0, + 'stiffness': 1.0, + 'stiffness_vel': 0.5, + 'dt': agent['dt'], + 'T': agent['T'], +} + +torque_cost = { + 'type': CostAction, + 'wu': 5e-5 / PR2_GAINS, +} + +fk_cost = { + 'type': CostFK, + 'target_end_effector': np.array([+1.6750e-01, -2.8100e-01, +3.0800e-01, + +1.6750e-01, -2.8100e-01, +1.0800e-01]), + 'wp': np.array([1, 1, 1, 1, 1, 1]), + 'l1': 0.1, + 'l2': 10.0, + 'alpha': 1e-5, +} + +algorithm['cost'] = { + 'type': CostSum, + 'costs': [torque_cost, fk_cost], + 'weights': [1.0, 1.0], +} + +algorithm['dynamics'] = { + 'type': DynamicsLRPrior, + 'regularization': 1e-6, + 'prior': { + 'type': DynamicsPriorGMM, + 'max_clusters': 20, + 'min_samples_per_cluster': 40, + 'max_samples': 20, + }, +} + +algorithm['traj_opt'] = { + 'type': TrajOptLQRPython, +} + +algorithm['policy_opt'] = {} + +config = { + 'iterations': algorithm['iterations'], + 'num_samples': 5, + 'verbose_trials': 1, + 'common': common, + 'agent': agent, + 'gui_on': True, + 'algorithm': algorithm, +} + +common['info'] = generate_experiment_info(config) diff --git a/python/gps/agent/config.py b/python/gps/agent/config.py index 9c2e18a6a..877dd549b 100755 --- a/python/gps/agent/config.py +++ b/python/gps/agent/config.py @@ -3,6 +3,9 @@ import numpy as np +from gps.proto.gps_pb2 import JOINT_ANGLES, JOINT_VELOCITIES, \ + END_EFFECTOR_POINTS, END_EFFECTOR_POINT_VELOCITIES, \ + END_EFFECTOR_POINT_JACOBIANS LOGGER = logging.getLogger(__name__) @@ -75,3 +78,26 @@ AGENT_BOX2D = { 'render': True, } + +AGENT_VREP = { + 'substeps': 1, + # V-REP remote api server port. + 'server_port': 19997, + # Starting index of various state components in data vector returned from + # V-REP. + 'sensor_idx': { + JOINT_ANGLES: 0, + JOINT_VELOCITIES: 7, + END_EFFECTOR_POINTS: 14, + END_EFFECTOR_POINT_VELOCITIES: 20, + END_EFFECTOR_POINT_JACOBIANS: 26, + }, + 'sensor_size': { + JOINT_ANGLES: 7, + JOINT_VELOCITIES: 7, + END_EFFECTOR_POINTS: 6, + END_EFFECTOR_POINT_VELOCITIES: 6, + END_EFFECTOR_POINT_JACOBIANS: 7 * 6, + }, +} + diff --git a/python/gps/agent/vrep/__init__.py b/python/gps/agent/vrep/__init__.py new file mode 100644 index 000000000..e69de29bb diff --git a/python/gps/agent/vrep/agent_vrep.py b/python/gps/agent/vrep/agent_vrep.py new file mode 100755 index 000000000..055885c6e --- /dev/null +++ b/python/gps/agent/vrep/agent_vrep.py @@ -0,0 +1,342 @@ +""" This file defines an agent for the V-REP simulator environment. + +To run this agent: + +1. Download V-REP. + +http://www.coppeliarobotics.com/downloads.html + +2. This program communicates with V-REP using its remote API: + +http://www.coppeliarobotics.com/helpFiles/en/remoteApiOverview.htm +http://www.coppeliarobotics.com/helpFiles/en/remoteApiClientSide.htm + +Help Python find the vrep module: + +$ export PYTHONPATH=$PYTHONPATH:path/to/vrep/programming/remoteApiBindings/python/python + +Copy the file remoteApi.so (or remoteApi.dll, or remoteApi.dylib) from the +V-REP installation folder to the root folder of your GPS installation: + +$ cp path/to/vrep/programming/remoteApiBindings/lib/lib/64Bit/remoteApi.so path/to/gps/ + +3. Launch V-REP. Leave it open prior to running this program. This program +automatically loads and runs V-REP simulations as needed. + +4. Run the experiment: + +$ python python/gps/gps_main.py vrep_example + +""" +import copy +import os +import sys +import time + +import numpy as np + +import vrep + +from gps.agent.agent import Agent +from gps.agent.agent_utils import generate_noise, setup +from gps.agent.config import AGENT_VREP +from gps.proto.gps_pb2 import JOINT_ANGLES, JOINT_VELOCITIES, \ + END_EFFECTOR_POINTS, END_EFFECTOR_POINT_VELOCITIES, \ + END_EFFECTOR_POINT_JACOBIANS, ACTION, RGB_IMAGE, RGB_IMAGE_SIZE, \ + CONTEXT_IMAGE, CONTEXT_IMAGE_SIZE + +from gps.sample.sample import Sample + +SERVER_STATE_SIMULATION_RUNNING = 1 +# Part of the V-REP agent implementation is in a child script attached to the +# following object. You can see it by opening the scene file in vrep_models +# folder and double clicking on the script icon next to the following object: +SCRIPT_OBJECT = 'Plane' + +np.set_printoptions(precision=3) +np.set_printoptions(suppress=True) + +class PrettyFloat(float): + def __repr__(self): + return "%0.3f" % self + +class AgentVREP(Agent): + """ + All communication between the algorithms and V-REP is done through + this class. + """ + def __init__(self, hyperparams): + config = copy.deepcopy(AGENT_VREP) + config.update(hyperparams) + Agent.__init__(self, config) + self._client_id = None + self._last_filename = None + self._sim_time = None + self._setup_conditions() + self._setup_vrep_client() + self._setup_world() + self._cached_vrep_state = None + + def _setup_conditions(self): + """ + Helper method for setting some hyperparameters that may vary by + condition. + """ + conds = self._hyperparams['conditions'] + for field in ('x0', 'x0var', 'pos_body_idx', 'pos_body_offset', + 'noisy_body_idx', 'noisy_body_var', 'filename'): + self._hyperparams[field] = setup(self._hyperparams[field], conds) + + def _setup_world(self): + """ + Helper method for handling setup of the MuJoCo world. + """ + # Initialize x0. + self.x0 = [] + for i in range(self._hyperparams['conditions']): + if END_EFFECTOR_POINTS in self.x_data_types: + # TODO: this assumes END_EFFECTOR_VELOCITIES is also in datapoints right? + self._load_world(i) + self._init_world(i) + self._fetch_vrep_state() + self.x0.append(self._get_X_from_vrep_state()) + self._close_world() + else: + self.x0.append(self._hyperparams['x0'][i]) + + def _setup_vrep_client(self): + """ + Helper method for setting up connection to V-REP remote api. + """ + self._client_id = vrep.simxStart('127.0.0.1', + self._hyperparams['server_port'], True, + True, 2000, 5) + if self._client_id == -1: + sys.exit('Could not connect to V-REP. Please launch V-REP first ' + 'and try again. There is no need to load any scene file ' + 'in V-REP.') + + def _vrep_book_keeping(self, last_fn, last_error_code): + """ + Helper method for debugging communication with V-REP. + """ + self._sim_time = vrep.simxGetLastCmdTime(self._client_id) + print '%d: %s: %s' % (self._sim_time, last_fn, last_error_code) + + def _load_world(self, cond): + """ + Helper method for handling loading of the V-REP world. + Args: + cond: Condition. + """ + filename = self._hyperparams['filename'][cond] + print filename + if filename == self._last_filename: + self._stop_simulation() + else: + self._last_filename = filename + filepath = os.path.abspath(filename) + print filepath + error_code = vrep.simxLoadScene(self._client_id, filepath, 0, + vrep.simx_opmode_blocking) + self._vrep_book_keeping('simxLoadScene', error_code) + + error_code = vrep.simxSynchronous(self._client_id, True) + self._vrep_book_keeping('simxSynchronous', error_code) + error_code = vrep.simxStartSimulation(self._client_id, + vrep.simx_opmode_blocking) + self._vrep_book_keeping('simxStartSimulation', error_code) + + def _stop_simulation(self): + """ + Helper method for stopping simulation. + """ + error_code = vrep.simxStopSimulation(self._client_id, + vrep.simx_opmode_blocking) + self._vrep_book_keeping('simxStopSimulation', error_code) + running = True + while running: + # Need to run some command so that simxGetInMessageInfo has access + # to fresh server state. + error_code, _ = vrep.simxGetPingTime(self._client_id) + error_code, server_state = vrep.simxGetInMessageInfo( + self._client_id, vrep.simx_headeroffset_server_state) + running = server_state & SERVER_STATE_SIMULATION_RUNNING + self._vrep_book_keeping('Server state', server_state) + + def _close_world(self): + """ + Helper method for handling setup of the MuJoCo world. + """ + self._stop_simulation() + error_code = vrep.simxCloseScene(self._client_id, + vrep.simx_opmode_blocking) + self._vrep_book_keeping('simxCloseScene', error_code) + self._last_filename = None + + def close_vrep_client(self): + """ + Helper method for closing connection to V-REP remote api. + """ + vrep.simxFinish(self._client_id) + + def _fetch_vrep_state(self): + """ + Helper method for getting the complete state from V-REP. + Args: + None. + Returns: + None. + """ + error_code, _, out_floats, _, _ = vrep.simxCallScriptFunction( + self._client_id, SCRIPT_OBJECT, + vrep.sim_scripttype_childscript, + 'state_function', [], [], [], bytearray(), + vrep.simx_opmode_blocking) + self._vrep_book_keeping('simxCallScriptFunction(state)', error_code) + self._cached_vrep_state = np.array(out_floats) + + def _transmit_vrep_action(self, vrep_U): + """ + Helper method for getting the complete state from V-REP. + Args: + vrep_U: Actions to send to V-REP. + Returns: + None. + """ + error_code, _, _, _, _ = vrep.simxCallScriptFunction( + self._client_id, SCRIPT_OBJECT, + vrep.sim_scripttype_childscript, + 'act_function', [], vrep_U, [], bytearray(), + vrep.simx_opmode_blocking) + self._vrep_book_keeping('simxCallScriptFunction(act)', error_code) + + def _get_sensor_from_vrep_state(self, sensor_name): + """ + Helper method for getting individual sensor values from cached V-REP + state. + Args: + sensor_name: Sensor to return state for. + Returns: + 1-D array containing requested sensor's value. + """ + regular_sensors = [JOINT_ANGLES, JOINT_VELOCITIES, + END_EFFECTOR_POINTS, END_EFFECTOR_POINT_VELOCITIES, \ + END_EFFECTOR_POINT_JACOBIANS] + image_sensors = [RGB_IMAGE, CONTEXT_IMAGE] + if sensor_name not in regular_sensors: + sys.exit('Support for vision not implemented.') + idx = self._hyperparams['sensor_idx'][sensor_name] + size = self._hyperparams['sensor_size'][sensor_name] + return self._cached_vrep_state[idx:idx + size] + + def _get_X_from_vrep_state(self): + """ + Helper method for getting the complete state from cached V-REP state. + Args: + sensor_name: Sensor to return state for. + Returns: + 1-D array containing the state. + """ + X = np.array([]) + for sensor_name in self._hyperparams['state_include']: + val = self._get_sensor_from_vrep_state(sensor_name) + X = np.concatenate((X, val)) + return X + + def sample(self, policy, condition, verbose=True, save=True, noisy=True): + """ + Runs a trial and constructs a new sample containing information + about the trial. + Args: + policy: Policy to to used in the trial. + condition: Which condition setup to run. + verbose: Whether or not to plot the trial. + save: Whether or not to store the trial into the samples. + noisy: Whether or not to use noise during sampling. + """ + self._load_world(condition) + self._init_world(condition) + + # Create new sample, populate first time step. + new_sample = Sample(self) + vrep_X = self._hyperparams['x0'][condition] + + U = np.zeros([self.T, self.dU]) + if noisy: + noise = generate_noise(self.T, self.dU, self._hyperparams) + else: + noise = np.zeros((self.T, self.dU)) + if np.any(self._hyperparams['x0var'][condition] > 0): + x0n = self._hyperparams['x0var'] * \ + np.random.randn(self._hyperparams['x0var'].shape) + vrep_X += x0n + noisy_body_idx = self._hyperparams['noisy_body_idx'][condition] + if noisy_body_idx.size > 0: + for i in range(len(noisy_body_idx)): + idx = noisy_body_idx[i] + var = self._hyperparams['noisy_body_var'][condition][i] + # self._model[condition]['body_pos'][idx, :] += \ + # var * np.random.randn(1, 3) + + # Initialize x0. + self._fetch_vrep_state() + self._set_sample(new_sample, -1) + + # Take the sample. + for t in range(self.T): + X_t = new_sample.get_X(t=t) + print 'X_t: %s' % map(PrettyFloat, X_t) + obs_t = new_sample.get_obs(t=t) + vrep_U = policy.act(X_t, obs_t, t, noise[t, :]) + print 'vrep_U: %s' % map(PrettyFloat, vrep_U) + U[t, :] = vrep_U + self._transmit_vrep_action(vrep_U / 10) + if (t + 1) < self.T: + for _ in range(self._hyperparams['substeps']): + error_code = vrep.simxSynchronousTrigger(self._client_id) + self._vrep_book_keeping('simxSynchronousTrigger', error_code) + #TODO: Some hidden state stuff will go here. + self._fetch_vrep_state() + self._set_sample(new_sample, t) + new_sample.set(ACTION, U) + if save: + self._samples[condition].append(new_sample) + self._close_world() + return new_sample + + def _init_world(self, condition): + """ + Set the world to a given state. + Args: + condition: Which condition to initialize. + """ + + # Initialize world. + x0 = self._hyperparams['x0'][condition] + idx = len(x0) // 2 + joint_angles = x0[:idx] + error_code, _, _, _, _ = vrep.simxCallScriptFunction( + self._client_id, SCRIPT_OBJECT, + vrep.sim_scripttype_childscript, + 'init_function', [], joint_angles, [], bytearray(), + vrep.simx_opmode_blocking) + self._vrep_book_keeping('simxCallScriptFunction(init)', error_code) + + def _set_sample(self, sample, t): + """ + Set the data for a sample for one time step. + Args: + sample: Sample object to set data for. + t: Time step to set for sample. + """ + for sensor_name in self._hyperparams['sensor_idx']: + sensor_value = self._get_sensor_from_vrep_state(sensor_name) + if sensor_name == END_EFFECTOR_POINT_JACOBIANS: + jac_shape = (self._hyperparams['sensor_size'][END_EFFECTOR_POINTS], + self._hyperparams['sensor_size'][JOINT_ANGLES]) + sensor_value = sensor_value.reshape(jac_shape) + print 'Jacobian: \n %s' % sensor_value + print 'Setting %s: %s' % (sensor_name, sensor_value) + sample.set(sensor_name, sensor_value, t=t + 1) + diff --git a/vrep_models/peg_insertion_torque.ttt b/vrep_models/peg_insertion_torque.ttt new file mode 100644 index 000000000..22682c3e1 Binary files /dev/null and b/vrep_models/peg_insertion_torque.ttt differ