Add V-REP agent and experiment.

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)

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,
+    },
+}
+