diff --git a/README.md b/README.md index 633ea189c..323ffe4ec 100644 --- a/README.md +++ b/README.md @@ -1,6 +1,22 @@ GPS ====== +Please check the mpc_gps branch for the reimplementation of MPC-GPS paper! + +## Demo +1. Hallway +``` +https://youtu.be/4h49wDTnrxw?list=PLeH0h5k_xzT5Rxuqkb2ZHuNJTIqAc76qN +``` + +2. 3 obstacle test +``` +https://youtu.be/xaNUwspDp2w +``` +--- + +====== + This code is a reimplementation of the guided policy search algorithm and LQG-based trajectory optimization, meant to help others understand, reuse, and build upon existing work. For full documentation, see [rll.berkeley.edu/gps](http://rll.berkeley.edu/gps). diff --git a/experiments/box2d_arm_example/hyperparams.py b/experiments/box2d_arm_example/hyperparams.py index ea3d8acbe..787f76ffd 100644 --- a/experiments/box2d_arm_example/hyperparams.py +++ b/experiments/box2d_arm_example/hyperparams.py @@ -37,6 +37,7 @@ 'data_files_dir': EXP_DIR + 'data_files/', 'log_filename': EXP_DIR + 'log.txt', 'conditions': 1, + 'use_mpc': True, } if not os.path.exists(common['data_files_dir']): @@ -46,7 +47,7 @@ 'type': AgentBox2D, 'target_state' : np.array([0, 0]), 'world' : ArmWorld, - 'render' : True, + 'render' : False, 'x0': np.array([0.75*np.pi, 0.5*np.pi, 0, 0, 0, 0, 0]), 'rk': 0, 'dt': 0.05, @@ -55,14 +56,20 @@ 'pos_body_idx': np.array([]), 'pos_body_offset': np.array([]), 'T': 100, + 'use_mpc': common['use_mpc'], + 'M': 5, 'sensor_dims': SENSOR_DIMS, 'state_include': [JOINT_ANGLES, JOINT_VELOCITIES, END_EFFECTOR_POINTS], 'obs_include': [], } +#if common['use_mpc']: +# agent['smooth_noise_var'] = 0.3 + algorithm = { 'type': AlgorithmTrajOpt, 'conditions': common['conditions'], + 'use_mpc': common['use_mpc'], } algorithm['init_traj_distr'] = { @@ -75,6 +82,16 @@ 'T': agent['T'], } +algorithm['init_mpc'] = { + 'type': init_lqr, + 'init_gains': np.zeros(SENSOR_DIMS[ACTION]), + 'init_acc': np.zeros(SENSOR_DIMS[ACTION]), + 'init_var': 0.1, + 'stiffness': 0.01, + 'dt': agent['dt'], + 'T': agent['M'], +} + action_cost = { 'type': CostAction, 'wu': np.array([1, 1]) diff --git a/experiments/box2d_pointmass_badmm_example/hyperparams.py b/experiments/box2d_pointmass_badmm_example/hyperparams.py new file mode 100644 index 000000000..094bf32d7 --- /dev/null +++ b/experiments/box2d_pointmass_badmm_example/hyperparams.py @@ -0,0 +1,163 @@ +""" Hyperparameters for Box2d Point Mass.""" +from __future__ import division + +import os.path +from datetime import datetime +import numpy as np + +from gps import __file__ as gps_filepath +from gps.agent.box2d.agent_box2d import AgentBox2D +from gps.agent.box2d.point_mass_world import PointMassWorld +from gps.algorithm.algorithm_badmm import AlgorithmBADMM +from gps.algorithm.cost.cost_state import CostState +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.policy.policy_prior_gmm import PolicyPriorGMM +from gps.algorithm.traj_opt.traj_opt_lqr_python import TrajOptLQRPython +from gps.algorithm.policy_opt.policy_opt_caffe import PolicyOptCaffe +from gps.algorithm.policy.lin_gauss_init import init_pd +from gps.gui.config import generate_experiment_info +from gps.proto.gps_pb2 import END_EFFECTOR_POINTS, END_EFFECTOR_POINT_VELOCITIES, ACTION, POSITION_NEAREST_OBSTACLE + +SENSOR_DIMS = { + END_EFFECTOR_POINTS: 3, + END_EFFECTOR_POINT_VELOCITIES: 3, + ACTION: 2 +} + +BASE_DIR = '/'.join(str.split(gps_filepath, '/')[:-2]) +EXP_DIR = BASE_DIR + '/../experiments/box2d_pointmass_badmm_example/' + + +common = { + 'experiment_name': 'box2d_pointmass_badmm_example' + '_' + \ + datetime.strftime(datetime.now(), '%m-%d-%y_%H-%M'), + 'experiment_dir': EXP_DIR, + 'data_files_dir': EXP_DIR + 'data_files/', + 'log_filename': EXP_DIR + 'log.txt', + 'conditions': 4, + 'use_mpc': True, +} + +if not os.path.exists(common['data_files_dir']): + os.makedirs(common['data_files_dir']) + +agent = { + 'type': AgentBox2D, + 'target_state' : np.array([5, 20, 0]), + "world" : PointMassWorld, + 'render' : False, + 'x0': [np.array([0, 5, 0, 0, 0, 0]), + np.array([0, 30, 0, 0, 0, 0]), + np.array([10, 5, 0, 0, 0, 0]), + np.array([10, 30, 0, 0, 0, 0]), + ], + 'rk': 0, + 'dt': 0.05, + 'substeps': 1, + 'conditions': common['conditions'], + 'pos_body_idx': np.array([]), + 'pos_body_offset': np.array([]), + 'T': 100, + 'use_mpc': common['use_mpc'], + 'M': 5, + 'sensor_dims': SENSOR_DIMS, + 'state_include': [END_EFFECTOR_POINTS, END_EFFECTOR_POINT_VELOCITIES], + 'obs_include': [END_EFFECTOR_POINTS, END_EFFECTOR_POINT_VELOCITIES], +} + +algorithm = { + 'type': AlgorithmBADMM, + 'conditions': common['conditions'], + 'iterations': 10, + 'lg_step_schedule': np.array([1e-4, 1e-3, 1e-2, 1e-2]), + 'policy_dual_rate': 0.2, + 'ent_reg_schedule': np.array([1e-3, 1e-3, 1e-2, 1e-1]), + 'fixed_lg_step': 3, + 'kl_step': 5.0, + 'min_step_mult': 0.01, + 'max_step_mult': 1.0, + 'sample_decrease_var': 0.05, + 'sample_increase_var': 0.1, +} + +algorithm['init_traj_distr'] = { + 'type': init_pd, + 'init_var': 5.0, + 'pos_gains': 0.0, + 'dQ': SENSOR_DIMS[ACTION], + 'dt': agent['dt'], + 'T': agent['T'], +} + +algorithm['init_mpc'] = { + 'type': init_pd, + 'init_var': 5.0, + 'pos_gains': 0.0, + 'dQ': SENSOR_DIMS[ACTION], + 'dt': agent['dt'], + 'T': agent['M'], +} + +action_cost = { + 'type': CostAction, + 'wu': np.array([5e-5, 5e-5]) +} + +state_cost = { + 'type': CostState, + 'data_types' : { + END_EFFECTOR_POINTS: { + 'wp': np.ones(SENSOR_DIMS[END_EFFECTOR_POINTS]), + 'target_state': agent["target_state"], + }, + }, +} + +algorithm['cost'] = { + 'type': CostSum, + 'costs': [action_cost, state_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'] = { + 'type': PolicyOptCaffe, + 'weights_file_prefix': EXP_DIR + 'policy', +} + +algorithm['policy_prior'] = { + 'type': PolicyPriorGMM, + 'max_clusters': 20, + 'min_samples_per_cluster': 40, + 'max_samples': 20, +} + +config = { + 'iterations': 10, + 'num_samples': 5, + 'verbose_trials': 5, + 'verbose_policy_trials': 0, + 'common': common, + 'agent': agent, + 'gui_on': True, + 'algorithm': algorithm, +} + +common['info'] = generate_experiment_info(config) diff --git a/experiments/box2d_pointmass_example/hyperparams.py b/experiments/box2d_pointmass_example/hyperparams.py index a53d7afba..c174177b1 100644 --- a/experiments/box2d_pointmass_example/hyperparams.py +++ b/experiments/box2d_pointmass_example/hyperparams.py @@ -9,6 +9,7 @@ from gps.agent.box2d.agent_box2d import AgentBox2D from gps.agent.box2d.point_mass_world import PointMassWorld from gps.algorithm.algorithm_traj_opt import AlgorithmTrajOpt +from gps.algorithm.cost.cost_obstacles import CostObstacle from gps.algorithm.cost.cost_state import CostState from gps.algorithm.cost.cost_action import CostAction from gps.algorithm.cost.cost_sum import CostSum @@ -16,7 +17,7 @@ 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_pd -from gps.proto.gps_pb2 import END_EFFECTOR_POINTS, END_EFFECTOR_POINT_VELOCITIES, ACTION +from gps.proto.gps_pb2 import END_EFFECTOR_POINTS, END_EFFECTOR_POINT_VELOCITIES, ACTION, POSITION_NEAREST_OBSTACLE from gps.gui.config import generate_experiment_info SENSOR_DIMS = { @@ -36,6 +37,7 @@ 'target_filename': EXP_DIR + 'target.npz', 'log_filename': EXP_DIR + 'log.txt', 'conditions': 1, + 'use_mpc': True, } if not os.path.exists(common['data_files_dir']): @@ -46,7 +48,7 @@ 'target_state' : np.array([5, 20, 0]), "world" : PointMassWorld, 'render' : False, - 'x0': np.array([0, 5, 0, 0, 0, 0]), + 'x0': [np.array([0, 5, 0, 0, 0, 0])], 'rk': 0, 'dt': 0.05, 'substeps': 1, @@ -54,14 +56,20 @@ 'pos_body_idx': np.array([]), 'pos_body_offset': np.array([]), 'T': 100, + 'use_mpc': common['use_mpc'], + 'M': 5, 'sensor_dims': SENSOR_DIMS, 'state_include': [END_EFFECTOR_POINTS, END_EFFECTOR_POINT_VELOCITIES], 'obs_include': [], } +#if common['use_mpc']: +# agent['smooth_noise_var'] = 1.0 + algorithm = { 'type': AlgorithmTrajOpt, 'conditions': common['conditions'], + 'use_mpc': common['use_mpc'], } algorithm['init_traj_distr'] = { @@ -73,6 +81,15 @@ 'T': agent['T'], } +algorithm['init_mpc'] = { + 'type': init_pd, + 'init_var': 5.0, + 'pos_gains': 0.0, + 'dQ': SENSOR_DIMS[ACTION], + 'dt': agent['dt'], + 'T': agent['M'], +} + action_cost = { 'type': CostAction, 'wu': np.array([5e-5, 5e-5]) diff --git a/experiments/box2d_pointmass_obstacle_badmm_example/hyperparams.py b/experiments/box2d_pointmass_obstacle_badmm_example/hyperparams.py new file mode 100644 index 000000000..698a2f8c1 --- /dev/null +++ b/experiments/box2d_pointmass_obstacle_badmm_example/hyperparams.py @@ -0,0 +1,189 @@ +""" Hyperparameters for Box2d Point Mass.""" +from __future__ import division + +import os.path +from datetime import datetime +import numpy as np + +from gps import __file__ as gps_filepath +from gps.agent.box2d.agent_box2d import AgentBox2D +from gps.agent.box2d.point_mass_world_obstacle import PointMassWorldObstacle +from gps.algorithm.algorithm_badmm import AlgorithmBADMM +from gps.algorithm.cost.cost_obstacles import CostObstacle +from gps.algorithm.cost.cost_state import CostState +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.policy.policy_prior_gmm import PolicyPriorGMM +from gps.algorithm.traj_opt.traj_opt_lqr_python import TrajOptLQRPython +from gps.algorithm.policy_opt.policy_opt_caffe import PolicyOptCaffe +from gps.algorithm.policy.lin_gauss_init import init_pd +from gps.gui.config import generate_experiment_info +from gps.proto.gps_pb2 import END_EFFECTOR_POINTS, END_EFFECTOR_POINT_VELOCITIES, ACTION, POSITION_NEAREST_OBSTACLE + +SENSOR_DIMS = { + END_EFFECTOR_POINTS: 3, + END_EFFECTOR_POINT_VELOCITIES: 3, + ACTION: 2 +} + +BASE_DIR = '/'.join(str.split(gps_filepath, '/')[:-2]) +EXP_DIR = BASE_DIR + '/../experiments/box2d_pointmass_obstacle_badmm_example/' + + +common = { + 'experiment_name': 'box2d_pointmass_obstacle_badmm_example' + '_' + \ + datetime.strftime(datetime.now(), '%m-%d-%y_%H-%M'), + 'experiment_dir': EXP_DIR, + 'data_files_dir': EXP_DIR + 'data_files/', + 'log_filename': EXP_DIR + 'log.txt', + 'conditions': 4, + 'use_mpc': False, +} + +if not os.path.exists(common['data_files_dir']): + os.makedirs(common['data_files_dir']) + +world_info = { + 'obstacles': [np.array([-5, 13, 4, 1]), + np.array([10, 13, 4, 1]), + np.array([-9, 20, 4, 1]), + np.array([15, 20, 4, 1]), + np.array([3, 20, 1.5, 1]), + ], +} + +x0_test = [np.array([0, 5, 0, 0, 0, 0]), + np.array([0, 8, 0, 0, 0, 0]), + np.array([0, 11, 0, 0, 0, 0]), + np.array([0, 14, 0, 0, 0, 0]), + ] + +agent = { + 'type': AgentBox2D, + 'target_state' : np.array([3, 35, 0]), + "world" : PointMassWorldObstacle, + 'world_info': world_info, + 'render' : False, + 'x0': [np.array([0, 5, 0, 0, 0, 0]), + np.array([2, 3, 0, 0, 0, 0]), + np.array([7, 3, 0, 0, 0, 0]), + np.array([-5, 3, 0, 0, 0, 0]), + ], + 'rk': 0, + 'dt': 0.05, + 'substeps': 1, + 'conditions': common['conditions'], + 'pos_body_idx': np.array([]), + 'pos_body_offset': np.array([]), + 'T': 100, + 'use_mpc': common['use_mpc'], + 'M': 5, + 'sensor_dims': SENSOR_DIMS, + 'state_include': [END_EFFECTOR_POINTS, END_EFFECTOR_POINT_VELOCITIES], + 'obs_include': [END_EFFECTOR_POINTS, END_EFFECTOR_POINT_VELOCITIES], +} + +algorithm = { + 'type': AlgorithmBADMM, + 'conditions': common['conditions'], + 'iterations': 10, + 'lg_step_schedule': np.array([1e-4, 1e-3, 1e-2, 1e-2]), + 'policy_dual_rate': 0.2, + 'ent_reg_schedule': np.array([1e-3, 1e-3, 1e-2, 1e-1]), + 'fixed_lg_step': 3, + 'kl_step': 5.0, + 'min_step_mult': 0.01, + 'max_step_mult': 1.0, + 'sample_decrease_var': 0.05, + 'sample_increase_var': 0.1, + 'use_mpc': common['use_mpc'], +} + +algorithm['init_traj_distr'] = { + 'type': init_pd, + 'init_var': 5.0, + 'pos_gains': 0.0, + 'dQ': SENSOR_DIMS[ACTION], + 'dt': agent['dt'], + 'T': agent['T'], +} + +algorithm['init_mpc'] = { + 'type': init_pd, + 'init_var': 5.0, + 'pos_gains': 0.0, + 'dQ': SENSOR_DIMS[ACTION], + 'dt': agent['dt'], + 'T': agent['M'], +} + +action_cost = { + 'type': CostAction, + 'wu': np.array([5e-5, 5e-5]) +} + +state_cost = { + 'type': CostState, + 'data_types' : { + END_EFFECTOR_POINTS: { + 'wp': np.ones(SENSOR_DIMS[END_EFFECTOR_POINTS]), + 'target_state': agent["target_state"], + }, + }, +} + +obstacle_cost = { + 'type': CostObstacle, + 'obstacle_type' : POSITION_NEAREST_OBSTACLE, + 'position_type': END_EFFECTOR_POINTS, + 'wp': np.ones(SENSOR_DIMS[END_EFFECTOR_POINTS]), + 'd_safe': 1.0 +} + +algorithm['cost'] = { + 'type': CostSum, + 'costs': [action_cost, state_cost, obstacle_cost], + 'weights': [1.0, 1.2, 10.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'] = { + 'type': PolicyOptCaffe, + 'weights_file_prefix': EXP_DIR + 'policy', +} + +algorithm['policy_prior'] = { + 'type': PolicyPriorGMM, + 'max_clusters': 20, + 'min_samples_per_cluster': 40, + 'max_samples': 20, +} + +config = { + 'iterations': algorithm['iterations'], + 'num_samples': 10, + 'verbose_trials': 5, + 'verbose_policy_trials': 0, + 'common': common, + 'agent': agent, + 'gui_on': True, + 'algorithm': algorithm, +} + +common['info'] = generate_experiment_info(config) diff --git a/experiments/box2d_pointmass_obstacle_example/hyperparams.py b/experiments/box2d_pointmass_obstacle_example/hyperparams.py new file mode 100644 index 000000000..2db8875d3 --- /dev/null +++ b/experiments/box2d_pointmass_obstacle_example/hyperparams.py @@ -0,0 +1,160 @@ +""" Hyperparameters for Box2d Point Mass.""" +from __future__ import division + +import os.path +from datetime import datetime +import numpy as np + +from gps import __file__ as gps_filepath +from gps.agent.box2d.agent_box2d import AgentBox2D +from gps.agent.box2d.point_mass_world_obstacle import PointMassWorldObstacle +from gps.algorithm.algorithm_traj_opt import AlgorithmTrajOpt +from gps.algorithm.cost.cost_obstacles import CostObstacle +from gps.algorithm.cost.cost_state import CostState +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_pd +from gps.proto.gps_pb2 import END_EFFECTOR_POINTS, END_EFFECTOR_POINT_VELOCITIES, ACTION, POSITION_NEAREST_OBSTACLE +from gps.gui.config import generate_experiment_info + +SENSOR_DIMS = { + END_EFFECTOR_POINTS: 3, + END_EFFECTOR_POINT_VELOCITIES: 3, + ACTION: 2 +} + +BASE_DIR = '/'.join(str.split(gps_filepath, '/')[:-2]) +EXP_DIR = BASE_DIR + '/../experiments/box2d_pointmass_obstacle_example/' + +common = { + 'experiment_name': 'box2d_pointmass_obstacle_example' + '_' + \ + 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, + 'use_mpc': False, +} + +if not os.path.exists(common['data_files_dir']): + os.makedirs(common['data_files_dir']) + +world_info = { + 'obstacles': [np.array([-5, 13, 4, 1]), + np.array([10, 13, 4, 1]), + np.array([-9, 20, 4, 1]), + np.array([15, 20, 4, 1]), + np.array([3, 20, 1.5, 1]), + ], +} + +agent = { + 'type': AgentBox2D, + 'target_state' : np.array([3, 35, 0]), + "world" : PointMassWorldObstacle, + 'world_info': world_info, + 'render' : False, + 'x0': [np.array([-3, 5, 0, 0, 0, 0])], + 'rk': 0, + 'dt': 0.05, + 'substeps': 1, + 'conditions': common['conditions'], + 'pos_body_idx': np.array([]), + 'pos_body_offset': np.array([]), + 'T': 100, + 'use_mpc': common['use_mpc'], + 'M': 5, + 'sensor_dims': SENSOR_DIMS, + 'state_include': [END_EFFECTOR_POINTS, END_EFFECTOR_POINT_VELOCITIES], + 'obs_include': [], +} + +#if common['use_mpc']: +# agent['smooth_noise_var'] = 1.0 + +algorithm = { + 'type': AlgorithmTrajOpt, + 'conditions': common['conditions'], + 'use_mpc': common['use_mpc'], +} + +algorithm['init_traj_distr'] = { + 'type': init_pd, + 'init_var': 5.0, + 'pos_gains': 0.0, + 'dQ': SENSOR_DIMS[ACTION], + 'dt': agent['dt'], + 'T': agent['T'], +} + +algorithm['init_mpc'] = { + 'type': init_pd, + 'init_var': 5.0, + 'pos_gains': 0.0, + 'dQ': SENSOR_DIMS[ACTION], + 'dt': agent['dt'], + 'T': agent['M'], +} + +action_cost = { + 'type': CostAction, + 'wu': np.array([5e-5, 5e-5]) +} + +state_cost = { + 'type': CostState, + 'data_types' : { + END_EFFECTOR_POINTS: { + 'wp': np.ones(SENSOR_DIMS[END_EFFECTOR_POINTS]), + 'target_state': agent["target_state"], + }, + }, +} + +obstacle_cost = { + 'type': CostObstacle, + 'obstacle_type' : POSITION_NEAREST_OBSTACLE, + 'position_type': END_EFFECTOR_POINTS, + 'wp': np.ones(SENSOR_DIMS[END_EFFECTOR_POINTS]), + 'd_safe': 1.0 +} + +algorithm['cost'] = { + 'type': CostSum, + 'costs': [action_cost, state_cost, obstacle_cost], + 'weights': [1.0, 1.2, 10.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': 15, + 'num_samples': 10, + 'common': common, + 'verbose_trials': 0, + 'agent': agent, + 'gui_on': True, + 'algorithm': algorithm, + 'dQ': algorithm['init_traj_distr']['dQ'], +} + +common['info'] = generate_experiment_info(config) diff --git a/experiments/turtlebot_badmm_example/hyperparams.py b/experiments/turtlebot_badmm_example/hyperparams.py new file mode 100644 index 000000000..b6445a3bb --- /dev/null +++ b/experiments/turtlebot_badmm_example/hyperparams.py @@ -0,0 +1,310 @@ +""" Hyperparameters for PR2 trajectory optimization experiment. """ +from __future__ import division + +from datetime import datetime +import os.path + +import numpy as np + +from gps import __file__ as gps_filepath +from gps.agent.ros.agent_turtlebot import AgentTurtlebot +from gps.algorithm.algorithm_badmm import AlgorithmBADMM +from gps.algorithm.cost.cost_fk import CostFK +from gps.algorithm.cost.cost_action import CostAction +from gps.algorithm.cost.cost_state import CostState +from gps.algorithm.cost.cost_obstacles import CostObstacle +from gps.algorithm.cost.cost_sum import CostSum +from gps.algorithm.cost.cost_utils import RAMP_LINEAR, RAMP_FINAL_ONLY +from gps.algorithm.dynamics.dynamics_lr_prior import DynamicsLRPrior +from gps.algorithm.dynamics.dynamics_prior_gmm import DynamicsPriorGMM +from gps.algorithm.policy.policy_prior_gmm import PolicyPriorGMM +from gps.algorithm.traj_opt.traj_opt_lqr_python import TrajOptLQRPython +from gps.algorithm.policy_opt.policy_opt_caffe import PolicyOptCaffe +from gps.algorithm.policy.lin_gauss_init import init_lqr, init_pd +from gps.gui.target_setup_gui import load_pose_from_npz +from gps.proto.gps_pb2 import MOBILE_POSITION, MOBILE_ORIENTATION, \ + MOBILE_VELOCITIES_LINEAR, MOBILE_VELOCITIES_ANGULAR, ACTION, \ + POSITION_NEAREST_OBSTACLE, MOBILE_RANGE_SENSOR +from gps.utility.general_utils import get_ee_points +from gps.gui.config import generate_experiment_info + +SENSOR_DIMS = { + MOBILE_POSITION: 3, + MOBILE_ORIENTATION: 4, + MOBILE_VELOCITIES_LINEAR: 3, + MOBILE_VELOCITIES_ANGULAR: 3, + MOBILE_RANGE_SENSOR: 30, + ACTION: 3 +} + +BASE_DIR = '/'.join(str.split(gps_filepath, '/')[:-2]) +EXP_DIR = BASE_DIR + '/../experiments/turtlebot_badmm_example/' + +# NOTE: This is odom pose. +# Default is in one_stacle.world +odom_pose = [1.5, 8.3, 0.] +x0s = [] +reset_conditions = [] + +# NOTE: This is map pose (The order of quaternion also different). +map_state = [np.array([4.0, 7.6, 0., # Position x, y, z + 1., 0., 0., 0., # Quaternion w, z, (x, y?) + 0., 0., 0., # Linear Velocities + 0., 0., 0.]), # Angular Velocities + np.array([4.0, 9.0, 0., # Position x, y, z + 1., 0., 0., 0., # Quaternion w, z, (x, y?) + 0., 0., 0., # Linear Velocities + 0., 0., 0.]), # Angular Velocities + #np.array([4.5, 7.2, 0., # Position x, y, z + #1., 0., 0., 0., # Quaternion w, z, (x, y?) + #0., 0., 0., # Linear Velocities + #0., 0., 0.]), # Angular Velocities + #np.array([5.5, 6.8, 0., # Position x, y, z + #1., 0., 0., 0., # Quaternion w, z, (x, y?) + #0., 0., 0., # Linear Velocities + #0., 0., 0.]), # Angular Velocities + #np.array([6.5, 6.8, 0., # Position x, y, z + #1., 0., 0., 0., # Quaternion w, z, (x, y?) + #0., 0., 0., # Linear Velocities + #0., 0., 0.]), # Angular Velocities + #np.array([4.0, 9.2, 0., # Position x, y, z + #1., 0., 0., 0., # Quaternion w, z, (x, y?) + #0., 0., 0., # Linear Velocities + #0., 0., 0.]), # Angular Velocities + #np.array([3.5, 8.3, 0., # Position x, y, z + #1., 0., 0., 0., # Quaternion w, z, (x, y?) + #0., 0., 0., # Linear Velocities + #0., 0., 0.]), # Angular Velocities + #np.array([3.5, 8.0, 0., # Position x, y, z + #1., 0., 0., 0., # Quaternion w, z, (x, y?) + #0., 0., 0., # Linear Velocities + #0., 0., 0.]), # Angular Velocities + #np.array([3.5, 8.6, 0., # Position x, y, z + #1., 0., 0., 0., # Quaternion w, z, (x, y?) + #0., 0., 0., # Linear Velocities + #0., 0., 0.]), # Angular Velocities + #np.array([3.5, 7.4, 0., # Position x, y, z + #1., 0., 0., 0., # Quaternion w, z, (x, y?) + #0., 0., 0., # Linear Velocities + #0., 0., 0.]), # Angular Velocities + #np.array([3.5, 9.2, 0., # Position x, y, z + #1., 0., 0., 0., # Quaternion w, z, (x, y?) + #0., 0., 0., # Linear Velocities + #0., 0., 0.]), # Angular Velocities + #np.array([4.5, 8.3, 0., # Position x, y, z + #1., 0., 0., 0., # Quaternion w, z, (x, y?) + #0., 0., 0., # Linear Velocities + #0., 0., 0.]), # Angular Velocities + #np.array([4.5, 8.0, 0., # Position x, y, z + #1., 0., 0., 0., # Quaternion w, z, (x, y?) + #0., 0., 0., # Linear Velocities + #0., 0., 0.]), # Angular Velocities + #np.array([4.5, 8.6, 0., # Position x, y, z + #1., 0., 0., 0., # Quaternion w, z, (x, y?) + #0., 0., 0., # Linear Velocities + #0., 0., 0.]), # Angular Velocities + #np.array([4.5, 7.4, 0., # Position x, y, z + #1., 0., 0., 0., # Quaternion w, z, (x, y?) + #0., 0., 0., # Linear Velocities + #0., 0., 0.]), # Angular Velocities + #np.array([4.5, 9.2, 0., # Position x, y, z + #1., 0., 0., 0., # Quaternion w, z, (x, y?) + #0., 0., 0., # Linear Velocities + #0., 0., 0.]), # Angular Velocities + ] +''' +# TEST 1_MPC_move_below +map_state = [np.array([3.5, 8.8, 0., # Position x, y, z + 1., 0., 0., 0., # Quaternion w, z, (x, y?) + 0., 0., 0., # Linear Velocities + 0., 0., 0.]), # Angular Velocities + np.array([3.5, 7.4, 0., # Position x, y, z + 1., 0., 0., 0., # Quaternion w, z, (x, y?) + 0., 0., 0., # Linear Velocities + 0., 0., 0.]), # Angular Velocities + np.array([5.5, 7.2, 0., # Position x, y, z + 1., 0., 0., 0., # Quaternion w, z, (x, y?) + 0., 0., 0., # Linear Velocities + 0., 0., 0.]), # Angular Velocities + ] +''' + +''' +map_state = [np.array([6.0, 6.0, 0., # Position x, y, z + 0.707, 0.707, 0., 0., # Quaternion w, z, (x, y?) + 0., 0., 0., # Linear Velocities + 0., 0., 0.]), # Angular Velocities + #np.array([3.5, 7.4, 0., # Position x, y, z + #1., 0., 0., 0., # Quaternion w, z, (x, y?) + #0., 0., 0., # Linear Velocities + #0., 0., 0.]), # Angular Velocities + #np.array([5.5, 7.2, 0., # Position x, y, z + #1., 0., 0., 0., # Quaternion w, z, (x, y?) + #0., 0., 0., # Linear Velocities + #0., 0., 0.]), # Angular Velocities + ] +''' + +map_state = [np.array([4.0, 8.6, 0., # Position x, y, z + 1., 0., 0., 0., # Quaternion w, z, (x, y?) + 0., 0., 0., # Linear Velocities + 0., 0., 0.]), # Angular Velocities + np.array([4.0, 9.0, 0., # Position x, y, z + 1., 0., 0., 0., # Quaternion w, z, (x, y?) + 0., 0., 0., # Linear Velocities + 0., 0., 0.]), # Angular Velocities + ] +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': len(map_state), + 'use_mpc': True, +} + +if not os.path.exists(common['data_files_dir']): + os.makedirs(common['data_files_dir']) + +for i in xrange(common['conditions']): + idx_pos = SENSOR_DIMS[MOBILE_POSITION] + idx_ori = SENSOR_DIMS[MOBILE_ORIENTATION] + + state = np.zeros(map_state[i].size) + state[:idx_pos] = map_state[i][:idx_pos] - odom_pose + + # odom state is independent to map state + state[idx_pos:idx_pos+idx_ori] = [0., 0., 0., 1.] + + x0s.append(state) + reset_conditions.append(map_state[i]) + +agent = { + 'type': AgentTurtlebot, + 'dt': 0.05, + 'conditions': common['conditions'], + 'T': 100, + 'x0': x0s, + 'use_mpc': common['use_mpc'], + 'M': 5, + 'reset_conditions': reset_conditions, + 'sensor_dims': SENSOR_DIMS, + 'state_include': [MOBILE_POSITION, MOBILE_ORIENTATION, \ + MOBILE_VELOCITIES_LINEAR, MOBILE_VELOCITIES_ANGULAR], + 'obs_include': [MOBILE_RANGE_SENSOR, MOBILE_ORIENTATION, \ + MOBILE_VELOCITIES_LINEAR, MOBILE_VELOCITIES_ANGULAR], +} + +algorithm = { + 'type': AlgorithmBADMM, + 'conditions': common['conditions'], + 'iterations': 10, + 'lg_step_schedule': np.array([1e-4, 1e-3, 1e-2, 1e-2]), + 'policy_dual_rate': 0.2, + 'ent_reg_schedule': np.array([1e-3, 1e-3, 1e-2, 1e-1]), + 'fixed_lg_step': 3, + 'kl_step': 5.0, + 'min_step_mult': 0.01, + 'max_step_mult': 1.0, + 'sample_decrease_var': 0.05, + 'sample_increase_var': 0.1, +} + +algorithm['init_traj_distr'] = { + 'type': init_pd, + 'init_var': 5.0, + 'pos_gains': 0.0, + 'dQ': SENSOR_DIMS[ACTION], + 'dt': agent['dt'], + 'T': agent['T'], +} + +algorithm['init_mpc'] = { + 'type': init_pd, + 'init_var': 5.0, + 'pos_gains': 0.0, + 'dQ': SENSOR_DIMS[ACTION], + 'dt': agent['dt'], + 'T': agent['M'], +} + + +action_cost = { + 'type': CostAction, + 'wu': np.ones(SENSOR_DIMS[ACTION])*5e-4 +} + +state_cost = { + 'type': CostState, + 'data_types' : { + MOBILE_ORIENTATION: { + 'wp': np.ones(SENSOR_DIMS[MOBILE_ORIENTATION])*1000., + 'target_state': np.array([0., 0., 0., 1.]), + }, + MOBILE_VELOCITIES_LINEAR: { + 'wp': np.ones(SENSOR_DIMS[MOBILE_VELOCITIES_LINEAR])*10000., + 'target_state': np.array([1., 0., 0.]), + }, + MOBILE_VELOCITIES_ANGULAR: { + 'wp': np.ones(SENSOR_DIMS[MOBILE_VELOCITIES_ANGULAR])*25., + 'target_state': np.array([0., 0., 0.]), + }, + }, +} + +obstacle_cost = { + 'type': CostObstacle, + 'obstacle_type' : POSITION_NEAREST_OBSTACLE, + 'position_type': MOBILE_POSITION, + 'wp': np.ones(SENSOR_DIMS[MOBILE_POSITION]), + 'd_safe': 0.4 +} + +algorithm['cost'] = { + 'type': CostSum, + 'costs': [action_cost, state_cost, obstacle_cost], + #'weights': [1.0, 1.0, 1000.0], + 'weights': [0.1, 0.1, 100.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'] = { + 'type': PolicyOptCaffe, + 'weights_file_prefix': EXP_DIR + 'policy', +} + +algorithm['policy_prior'] = { + 'type': PolicyPriorGMM, + 'max_clusters': 20, + 'min_samples_per_cluster': 40, + 'max_samples': 20, +} + +config = { + 'iterations': algorithm['iterations'], + 'common': common, + 'verbose_trials': 0, + 'verbose_policy_trials': 1, + 'agent': agent, + 'gui_on': True, + 'algorithm': algorithm, + 'num_samples': 5, +} + +common['info'] = generate_experiment_info(config) diff --git a/experiments/turtlebot_example/hyperparams.py b/experiments/turtlebot_example/hyperparams.py new file mode 100644 index 000000000..e9142ab82 --- /dev/null +++ b/experiments/turtlebot_example/hyperparams.py @@ -0,0 +1,183 @@ +""" Hyperparameters for PR2 trajectory optimization experiment. """ +from __future__ import division + +from datetime import datetime +import os.path + +import numpy as np + +from gps import __file__ as gps_filepath +from gps.agent.ros.agent_turtlebot import AgentTurtlebot +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_state import CostState +from gps.algorithm.cost.cost_obstacles import CostObstacle +from gps.algorithm.cost.cost_sum import CostSum +from gps.algorithm.cost.cost_utils import RAMP_LINEAR, RAMP_FINAL_ONLY +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, init_pd +from gps.gui.target_setup_gui import load_pose_from_npz +from gps.proto.gps_pb2 import MOBILE_POSITION, MOBILE_ORIENTATION, \ + MOBILE_VELOCITIES_LINEAR, MOBILE_VELOCITIES_ANGULAR, ACTION, \ + POSITION_NEAREST_OBSTACLE +from gps.utility.general_utils import get_ee_points +from gps.gui.config import generate_experiment_info + +SENSOR_DIMS = { + MOBILE_POSITION: 3, + MOBILE_ORIENTATION: 4, + MOBILE_VELOCITIES_LINEAR: 3, + MOBILE_VELOCITIES_ANGULAR: 3, + ACTION: 3 +} + +BASE_DIR = '/'.join(str.split(gps_filepath, '/')[:-2]) +EXP_DIR = BASE_DIR + '/../experiments/turtlebot_example/' + +# NOTE: This is odom pose. +# Default is in one_stacle.world + +odom_pose = [1.5, 8.3, 0.] +x0s = [] +reset_conditions = [] +# NOTE: This is map pose (The order of quaternion also different). +map_state = [np.array([3.5, 8.3, 0., # Position x, y, z + 1., 0., 0., 0., # Quaternion w, z, (x, y?) + 0., 0., 0., # Linear Velocities + 0., 0., 0.])] # Angular Velocities +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, + 'use_mpc': True, +} + +if not os.path.exists(common['data_files_dir']): + os.makedirs(common['data_files_dir']) + +for i in xrange(common['conditions']): + idx_pos = SENSOR_DIMS[MOBILE_POSITION] + idx_ori = SENSOR_DIMS[MOBILE_ORIENTATION] + + state = np.zeros(map_state[i].size) + state[:idx_pos] = map_state[i][:idx_pos] - odom_pose + + # odom state is independent to map state + state[idx_pos:idx_pos+idx_ori] = [0., 0., 0., 1.] + + x0s.append(state) + reset_conditions.append(map_state[i]) + +agent = { + 'type': AgentTurtlebot, + 'dt': 0.05, + 'conditions': common['conditions'], + 'T': 100, + 'x0': x0s, + 'use_mpc': common['use_mpc'], + 'M': 5, + 'reset_conditions': reset_conditions, + 'sensor_dims': SENSOR_DIMS, + 'state_include': [MOBILE_POSITION, MOBILE_ORIENTATION, \ + MOBILE_VELOCITIES_LINEAR, MOBILE_VELOCITIES_ANGULAR], + 'obs_include': [], +} + +algorithm = { + 'type': AlgorithmTrajOpt, + 'conditions': common['conditions'], + 'use_mpc': common['use_mpc'], + 'iterations': 10, +} + +algorithm['init_traj_distr'] = { + 'type': init_pd, + 'init_var': 5.0, + 'pos_gains': 0.0, + 'dQ': SENSOR_DIMS[ACTION], + 'dt': agent['dt'], + 'T': agent['T'], +} + +algorithm['init_mpc'] = { + 'type': init_pd, + 'init_var': 5.0, + 'pos_gains': 0.0, + 'dQ': SENSOR_DIMS[ACTION], + 'dt': agent['dt'], + 'T': agent['M'], +} + +action_cost = { + 'type': CostAction, + 'wu': np.ones(SENSOR_DIMS[ACTION])*5e-3 +} + +state_cost = { + 'type': CostState, + 'data_types' : { + MOBILE_ORIENTATION: { + 'wp': np.ones(SENSOR_DIMS[MOBILE_ORIENTATION])*100., + 'target_state': np.array([0., 0., 0., 1.]), + }, + MOBILE_VELOCITIES_LINEAR: { + 'wp': np.ones(SENSOR_DIMS[MOBILE_VELOCITIES_LINEAR])*100.0, + 'target_state': np.array([1.0, 0., 0.]), + }, + MOBILE_VELOCITIES_ANGULAR: { + 'wp': np.ones(SENSOR_DIMS[MOBILE_VELOCITIES_ANGULAR])*2.5, + 'target_state': np.array([0., 0., 0.]), + }, + }, +} + +obstacle_cost = { + 'type': CostObstacle, + 'obstacle_type' : POSITION_NEAREST_OBSTACLE, + 'position_type': MOBILE_POSITION, + 'wp': np.ones(SENSOR_DIMS[MOBILE_POSITION]), + 'd_safe': 0.4 +} + +algorithm['cost'] = { + 'type': CostSum, + 'costs': [action_cost, state_cost, obstacle_cost], + 'weights': [1.0, 1.0, 25.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'], + 'common': common, + 'verbose_trials': 0, + 'agent': agent, + 'gui_on': True, + 'algorithm': algorithm, + 'num_samples': 5, +} + +common['info'] = generate_experiment_info(config) diff --git a/experiments/turtlebot_hallway_badmm_example/hyperparams.py b/experiments/turtlebot_hallway_badmm_example/hyperparams.py new file mode 100644 index 000000000..2d9108105 --- /dev/null +++ b/experiments/turtlebot_hallway_badmm_example/hyperparams.py @@ -0,0 +1,234 @@ +""" Hyperparameters for PR2 trajectory optimization experiment. """ +from __future__ import division + +from datetime import datetime +import os.path + +import numpy as np + +from gps import __file__ as gps_filepath +from gps.agent.ros.agent_turtlebot import AgentTurtlebot +from gps.algorithm.algorithm_badmm import AlgorithmBADMM +from gps.algorithm.cost.cost_fk import CostFK +from gps.algorithm.cost.cost_action import CostAction +from gps.algorithm.cost.cost_state import CostState +from gps.algorithm.cost.cost_obstacles import CostObstacle +from gps.algorithm.cost.cost_sum import CostSum +from gps.algorithm.cost.cost_utils import RAMP_LINEAR, RAMP_FINAL_ONLY +from gps.algorithm.dynamics.dynamics_lr_prior import DynamicsLRPrior +from gps.algorithm.dynamics.dynamics_prior_gmm import DynamicsPriorGMM +from gps.algorithm.policy.policy_prior_gmm import PolicyPriorGMM +from gps.algorithm.traj_opt.traj_opt_lqr_python import TrajOptLQRPython +from gps.algorithm.policy_opt.policy_opt_caffe import PolicyOptCaffe +from gps.algorithm.policy.lin_gauss_init import init_lqr, init_pd +from gps.gui.target_setup_gui import load_pose_from_npz +from gps.proto.gps_pb2 import MOBILE_POSITION, MOBILE_ORIENTATION, \ + MOBILE_VELOCITIES_LINEAR, MOBILE_VELOCITIES_ANGULAR, ACTION, \ + POSITION_NEAREST_OBSTACLE, MOBILE_RANGE_SENSOR +from gps.utility.general_utils import get_ee_points +from gps.gui.config import generate_experiment_info + +SENSOR_DIMS = { + MOBILE_POSITION: 3, + MOBILE_ORIENTATION: 4, + MOBILE_VELOCITIES_LINEAR: 3, + MOBILE_VELOCITIES_ANGULAR: 3, + MOBILE_RANGE_SENSOR: 30, + ACTION: 3 +} + +BASE_DIR = '/'.join(str.split(gps_filepath, '/')[:-2]) +EXP_DIR = BASE_DIR + '/../experiments/turtlebot_hallway_badmm_example/' + +# NOTE: This is odom pose. +# Default is in one_stacle.world +odom_pose = [3.5, 10.3, 0.] +#odom_pose = [3.5, 11.25, 0.] # Test hallway bend +x0s = [] +reset_conditions = [] + +# NOTE: This is map pose (The order of quaternion also different). +map_state = [#np.array([5.5, 9.2, 0., # Position x, y, z + #1., 0., 0., 0., # Quaternion w, z, (x, y?) + #0., 0., 0., # Linear Velocities + #0., 0., 0.]), # Angular Velocities + np.array([3.5, 9.55, 0., # Position x, y, z + 1., 0., 0., 0., # Quaternion w, z, (x, y?) + 0., 0., 0., # Linear Velocities + 0., 0., 0.]), # Angular Velocities + np.array([3.5, 10.3, 0., # Position x, y, z + 1., 0., 0., 0., # Quaternion w, z, (x, y?) + 0., 0., 0., # Linear Velocities + 0., 0., 0.]), # Angular Velocities + np.array([3.5, 11.2, 0., # Position x, y, z + 1., 0., 0., 0., # Quaternion w, z, (x, y?) + 0., 0., 0., # Linear Velocities + 0., 0., 0.]), # Angular Velocities + #np.array([5.5, 11.5, 0., # Position x, y, z + #1., 0., 0., 0., # Quaternion w, z, (x, y?) + #0., 0., 0., # Linear Velocities + #0., 0., 0.]), # Angular Velocities + ] + +# Test Hallway bend +''' +map_state = [np.array([8.5, 11.2, 0., # Position x, y, z + 1., 0., 0., 0., # Quaternion w, z, (x, y?) + 0., 0., 0., # Linear Velocities + 0., 0., 0.]), # Angular Velocities + ] +''' +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': len(map_state), + 'use_mpc': True, +} + +if not os.path.exists(common['data_files_dir']): + os.makedirs(common['data_files_dir']) + +for i in xrange(common['conditions']): + idx_pos = SENSOR_DIMS[MOBILE_POSITION] + idx_ori = SENSOR_DIMS[MOBILE_ORIENTATION] + + state = np.zeros(map_state[i].size) + state[:idx_pos] = map_state[i][:idx_pos] - odom_pose + + # odom state is independent to map state + state[idx_pos:idx_pos+idx_ori] = [0., 0., 0., 1.] + + x0s.append(state) + reset_conditions.append(map_state[i]) + +agent = { + 'type': AgentTurtlebot, + 'dt': 0.05, + 'conditions': common['conditions'], + 'T': 150, + 'x0': x0s, + 'use_mpc': common['use_mpc'], + 'M': 10, + 'reset_conditions': reset_conditions, + 'sensor_dims': SENSOR_DIMS, + 'state_include': [MOBILE_POSITION, MOBILE_ORIENTATION, \ + MOBILE_VELOCITIES_LINEAR, MOBILE_VELOCITIES_ANGULAR], + 'obs_include': [MOBILE_RANGE_SENSOR, MOBILE_ORIENTATION, \ + MOBILE_VELOCITIES_LINEAR, MOBILE_VELOCITIES_ANGULAR], +} + +algorithm = { + 'type': AlgorithmBADMM, + 'conditions': common['conditions'], + 'iterations': 10, + 'lg_step_schedule': np.array([1e-4, 1e-3, 1e-2, 1e-2]), + 'policy_dual_rate': 0.2, + 'ent_reg_schedule': np.array([1e-3, 1e-3, 1e-2, 1e-1]), + 'fixed_lg_step': 3, + 'kl_step': 5.0, + 'min_step_mult': 0.01, + 'max_step_mult': 1.0, + 'sample_decrease_var': 0.05, + 'sample_increase_var': 0.1, +} + +algorithm['init_traj_distr'] = { + 'type': init_pd, + 'init_var': 5.0, + 'pos_gains': 0.0, + 'dQ': SENSOR_DIMS[ACTION], + 'dt': agent['dt'], + 'T': agent['T'], +} + +algorithm['init_mpc'] = { + 'type': init_pd, + 'init_var': 5.0, + 'pos_gains': 0.0, + 'dQ': SENSOR_DIMS[ACTION], + 'dt': agent['dt'], + 'T': agent['M'], +} + + +action_cost = { + 'type': CostAction, + 'wu': np.ones(SENSOR_DIMS[ACTION])*5e-5 +} + +state_cost = { + 'type': CostState, + 'data_types' : { + MOBILE_ORIENTATION: { + 'wp': np.ones(SENSOR_DIMS[MOBILE_ORIENTATION])*100., + 'target_state': np.array([0., 0., 0., 1.]), + }, + MOBILE_VELOCITIES_LINEAR: { + 'wp': np.ones(SENSOR_DIMS[MOBILE_VELOCITIES_LINEAR])*100., + 'target_state': np.array([1.0, 0., 0.]), + }, + MOBILE_VELOCITIES_ANGULAR: { + 'wp': np.ones(SENSOR_DIMS[MOBILE_VELOCITIES_ANGULAR])*2.5, + 'target_state': np.array([0., 0., 0.]), + }, + }, +} + +obstacle_cost = { + 'type': CostObstacle, + 'obstacle_type' : POSITION_NEAREST_OBSTACLE, + 'position_type': MOBILE_POSITION, + 'wp': np.ones(SENSOR_DIMS[MOBILE_POSITION]), + 'd_safe': 0.4 +} + +algorithm['cost'] = { + 'type': CostSum, + 'costs': [action_cost, state_cost, obstacle_cost], + 'weights': [1.0, 1.0, 25.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'] = { + 'type': PolicyOptCaffe, + 'weights_file_prefix': EXP_DIR + 'policy', +} + +algorithm['policy_prior'] = { + 'type': PolicyPriorGMM, + 'max_clusters': 20, + 'min_samples_per_cluster': 40, + 'max_samples': 20, +} + +config = { + 'iterations': algorithm['iterations'], + 'common': common, + 'verbose_trials': 0, + 'verbose_policy_trials': 1, + 'agent': agent, + 'gui_on': True, + 'algorithm': algorithm, + 'num_samples': 5, +} + +common['info'] = generate_experiment_info(config) diff --git a/experiments/turtlebot_hallway_pigps_example/hyperparams.py b/experiments/turtlebot_hallway_pigps_example/hyperparams.py new file mode 100644 index 000000000..56a49e49c --- /dev/null +++ b/experiments/turtlebot_hallway_pigps_example/hyperparams.py @@ -0,0 +1,277 @@ +""" Hyperparameters for PR2 trajectory optimization experiment. """ +from __future__ import division + +from datetime import datetime +import os.path + +import numpy as np + +from gps import __file__ as gps_filepath +from gps.agent.ros.agent_mpepc_turtlebot import AgentTurtlebot +from gps.algorithm.algorithm_pigps import AlgorithmPIGPS +from gps.algorithm.algorithm_pigps import AlgorithmMDGPS +from gps.algorithm.cost.cost_fk import CostFK +from gps.algorithm.cost.cost_action import CostAction +from gps.algorithm.cost.cost_state import CostState +from gps.algorithm.cost.cost_obstacles import CostObstacle +from gps.algorithm.cost.cost_sum import CostSum +from gps.algorithm.cost.cost_utils import RAMP_LINEAR, RAMP_FINAL_ONLY +from gps.algorithm.dynamics.dynamics_lr_prior import DynamicsLRPrior +from gps.algorithm.dynamics.dynamics_prior_gmm import DynamicsPriorGMM +from gps.algorithm.policy.policy_prior_gmm import PolicyPriorGMM +from gps.algorithm.traj_opt.traj_opt_pi2 import TrajOptPI2 +from gps.algorithm.policy_opt.policy_opt_caffe import PolicyOptCaffe +from gps.algorithm.policy.lin_gauss_init import init_lqr, init_pd +from gps.gui.target_setup_gui import load_pose_from_npz +from gps.proto.gps_pb2 import MOBILE_POSITION, MOBILE_ORIENTATION, \ + MOBILE_VELOCITIES_LINEAR, MOBILE_VELOCITIES_ANGULAR, ACTION, \ + POSITION_NEAREST_OBSTACLE, MOBILE_RANGE_SENSOR +from gps.utility.general_utils import get_ee_points +from gps.gui.config import generate_experiment_info + +SENSOR_DIMS = { + MOBILE_POSITION: 3, + MOBILE_ORIENTATION: 4, + MOBILE_VELOCITIES_LINEAR: 3, + MOBILE_VELOCITIES_ANGULAR: 3, + MOBILE_RANGE_SENSOR: 30, + ACTION: 3 +} + +BASE_DIR = '/'.join(str.split(gps_filepath, '/')[:-2]) +EXP_DIR = BASE_DIR + '/../experiments/turtlebot_hallway_pigps_example/' + +# NOTE: This is odom pose. +# Default is in one_stacle.world +odom_pose = [3.5, 10.3, 0.] +#odom_pose = [3.5, 11.25, 0.] # Test hallway bend +x0s = [] +reset_conditions = [] + +# NOTE: This is map pose (The order of quaternion also different). +map_state = [#np.array([5.5, 9.2, 0., # Position x, y, z + #1., 0., 0., 0., # Quaternion w, z, (x, y?) + #0., 0., 0., # Linear Velocities + #0., 0., 0.]), # Angular Velocities + np.array([3.5, 9.55, 0., # Position x, y, z + 1., 0., 0., 0., # Quaternion w, z, (x, y?) + 0., 0., 0., # Linear Velocities + 0., 0., 0.]), # Angular Velocities + np.array([3.5, 10.3, 0., # Position x, y, z + 1., 0., 0., 0., # Quaternion w, z, (x, y?) + 0., 0., 0., # Linear Velocities + 0., 0., 0.]), # Angular Velocities + np.array([3.5, 11.2, 0., # Position x, y, z + 1., 0., 0., 0., # Quaternion w, z, (x, y?) + 0., 0., 0., # Linear Velocities + 0., 0., 0.]), # Angular Velocities + #np.array([5.5, 11.5, 0., # Position x, y, z + #1., 0., 0., 0., # Quaternion w, z, (x, y?) + #0., 0., 0., # Linear Velocities + #0., 0., 0.]), # Angular Velocities + ] + +goal_conditions = [#np.array([5.5, 9.2, 0., # Position x, y, z + #1., 0., 0., 0., # Quaternion w, z, (x, y?) + #0., 0., 0., # Linear Velocities + #0., 0., 0.]), # Angular Velocities + np.array([27.5, 12.3, 0., # Position x, y, z + 1., 0., 0., 0., # Quaternion w, z, (x, y?) + 0., 0., 0., # Linear Velocities + 0., 0., 0.]), # Angular Velocities + np.array([27.5, 12.6, 0., # Position x, y, z + 1., 0., 0., 0., # Quaternion w, z, (x, y?) + 0., 0., 0., # Linear Velocities + 0., 0., 0.]), # Angular Velocities + np.array([27.5, 12.9, 0., # Position x, y, z + 1., 0., 0., 0., # Quaternion w, z, (x, y?) + 0., 0., 0., # Linear Velocities + 0., 0., 0.]), # Angular Velocities + #np.array([5.5, 11.5, 0., # Position x, y, z + #1., 0., 0., 0., # Quaternion w, z, (x, y?) + #0., 0., 0., # Linear Velocities + #0., 0., 0.]), # Angular Velocities +] + +# Test Hallway bend +''' +map_state = [np.array([8.5, 11.2, 0., # Position x, y, z + 1., 0., 0., 0., # Quaternion w, z, (x, y?) + 0., 0., 0., # Linear Velocities + 0., 0., 0.]), # Angular Velocities + ] +''' +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': len(map_state), + 'use_mpc': False, +} + +if not os.path.exists(common['data_files_dir']): + os.makedirs(common['data_files_dir']) + +for i in xrange(common['conditions']): + idx_pos = SENSOR_DIMS[MOBILE_POSITION] + idx_ori = SENSOR_DIMS[MOBILE_ORIENTATION] + + state = np.zeros(map_state[i].size) + state[:idx_pos] = map_state[i][:idx_pos] - odom_pose + + # odom state is independent to map state + state[idx_pos:idx_pos+idx_ori] = [0., 0., 0., 1.] + + x0s.append(state) + reset_conditions.append(map_state[i]) + +agent = { + 'type': AgentTurtlebot, + 'dt': 0.05, + 'conditions': common['conditions'], + 'T': 150, + 'x0': x0s, + 'use_mpc': common['use_mpc'], + 'M': 10, + 'reset_conditions': reset_conditions, + 'goal_conditions': goal_conditions, + 'sensor_dims': SENSOR_DIMS, + 'state_include': [MOBILE_POSITION, MOBILE_ORIENTATION, \ + MOBILE_VELOCITIES_LINEAR, MOBILE_VELOCITIES_ANGULAR], + 'obs_include': [MOBILE_RANGE_SENSOR, MOBILE_ORIENTATION, \ + MOBILE_VELOCITIES_LINEAR, MOBILE_VELOCITIES_ANGULAR], +} + +# algorithm = { +# 'type': AlgorithmBADMM, +# 'conditions': common['conditions'], +# 'iterations': 10, +# 'lg_step_schedule': np.array([1e-4, 1e-3, 1e-2, 1e-2]), +# 'policy_dual_rate': 0.2, +# 'ent_reg_schedule': np.array([1e-3, 1e-3, 1e-2, 1e-1]), +# 'fixed_lg_step': 3, +# 'kl_step': 5.0, +# 'min_step_mult': 0.01, +# 'max_step_mult': 1.0, +# 'sample_decrease_var': 0.05, +# 'sample_increase_var': 0.1, +# } + +algorithm = { + 'type': AlgorithmPIGPS, + 'conditions': common['conditions'], + 'policy_sample_mode': 'replace', + 'sample_on_policy': True, +} + +algorithm['init_traj_distr'] = { + 'type': init_pd, + 'init_var': 1.0, + 'pos_gains': 0.0, + 'dQ': SENSOR_DIMS[ACTION], + 'dt': agent['dt'], + 'T': agent['T'], +} + +algorithm['init_mpc'] = { + 'type': init_pd, + 'init_var': 5.0, + 'pos_gains': 0.0, + 'dQ': SENSOR_DIMS[ACTION], + 'dt': agent['dt'], + 'T': agent['M'], +} + + +action_cost = { + 'type': CostAction, + 'wu': np.ones(SENSOR_DIMS[ACTION])*5e-5 +} + +state_cost = { + 'type': CostState, + 'data_types' : { + MOBILE_ORIENTATION: { + 'wp': np.ones(SENSOR_DIMS[MOBILE_ORIENTATION])*100., + 'target_state': np.array([0., 0., 0., 1.]), + }, + MOBILE_VELOCITIES_LINEAR: { + 'wp': np.ones(SENSOR_DIMS[MOBILE_VELOCITIES_LINEAR])*100., + 'target_state': np.array([1.5, 0., 0.]), + }, + MOBILE_VELOCITIES_ANGULAR: { + 'wp': np.ones(SENSOR_DIMS[MOBILE_VELOCITIES_ANGULAR])*2.5, + 'target_state': np.array([0., 0., 0.]), + }, + }, +} + +obstacle_cost = { + 'type': CostObstacle, + 'obstacle_type' : POSITION_NEAREST_OBSTACLE, + 'position_type': MOBILE_POSITION, + 'wp': np.ones(SENSOR_DIMS[MOBILE_POSITION]), + 'd_safe': 1.0 +} + +algorithm['cost'] = { + 'type': CostSum, + 'costs': [action_cost, state_cost, obstacle_cost], + 'weights': [1.0, 1.0, 50.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['traj_opt'] = { + 'type': TrajOptPI2, + 'kl_threshold': 2.0, + 'covariance_damping': 2.0, + 'min_temperature': 0.001, +} + +algorithm['policy_opt'] = { + 'type': PolicyOptCaffe, + 'weights_file_prefix': EXP_DIR + 'policy', + 'iterations': 10000, + 'network_arch_params': { + 'n_layers': 2, + 'dim_hidden': [20], + }, +} + +algorithm['policy_prior'] = { + 'type': PolicyPriorGMM, + 'max_clusters': 20, + 'min_samples_per_cluster': 40, + 'max_samples': 20, +} + +config = { + 'iterations': 30, + 'num_samples': 20, + 'common': common, + 'verbose_trials': 0, + 'verbose_policy_trials': 1, + 'agent': agent, + 'gui_on': True, + 'algorithm': algorithm, +} + +common['info'] = generate_experiment_info(config) diff --git a/experiments/turtlebot_mpepc_hallway_badmm_example/hyperparams.py b/experiments/turtlebot_mpepc_hallway_badmm_example/hyperparams.py new file mode 100644 index 000000000..9df2771fd --- /dev/null +++ b/experiments/turtlebot_mpepc_hallway_badmm_example/hyperparams.py @@ -0,0 +1,277 @@ +""" Hyperparameters for PR2 trajectory optimization experiment. """ +from __future__ import division + +from datetime import datetime +import os.path + +import numpy as np + +from gps import __file__ as gps_filepath +from gps.agent.ros.agent_mpepc_turtlebot import AgentMPEPCTurtlebot +from gps.algorithm.algorithm_badmm import AlgorithmBADMM +from gps.algorithm.cost.cost_fk import CostFK +from gps.algorithm.cost.cost_action import CostAction +from gps.algorithm.cost.cost_state import CostState +from gps.algorithm.cost.cost_obstacles import CostObstacle +from gps.algorithm.cost.cost_sum import CostSum +from gps.algorithm.cost.cost_utils import RAMP_LINEAR, RAMP_FINAL_ONLY +from gps.algorithm.dynamics.dynamics_lr_prior import DynamicsLRPrior +from gps.algorithm.dynamics.dynamics_prior_gmm import DynamicsPriorGMM +from gps.algorithm.policy.policy_prior_gmm import PolicyPriorGMM +from gps.algorithm.traj_opt.traj_opt_pi2 import TrajOptPI2 +from gps.algorithm.traj_opt.traj_opt_lqr_python import TrajOptLQRPython +from gps.algorithm.policy_opt.policy_opt_caffe import PolicyOptCaffe +from gps.algorithm.policy.lin_gauss_init import init_lqr, init_pd +from gps.gui.target_setup_gui import load_pose_from_npz +from gps.proto.gps_pb2 import MOBILE_POSITION, MOBILE_ORIENTATION, \ + MOBILE_VELOCITIES_LINEAR, MOBILE_VELOCITIES_ANGULAR, ACTION, \ + POSITION_NEAREST_OBSTACLE, POTENTIAL_SCORE, MOBILE_RANGE_SENSOR +from gps.utility.general_utils import get_ee_points +from gps.gui.config import generate_experiment_info + +SENSOR_DIMS = { + MOBILE_POSITION: 3, + MOBILE_ORIENTATION: 4, + MOBILE_VELOCITIES_LINEAR: 3, + MOBILE_VELOCITIES_ANGULAR: 3, + MOBILE_RANGE_SENSOR: 30, + ACTION: 4 +} + +BASE_DIR = '/'.join(str.split(gps_filepath, '/')[:-2]) +EXP_DIR = BASE_DIR + '/../experiments/turtlebot_mpepc_hallway_badmm_example/' + +# NOTE: This is odom pose. +# Default is in one_stacle.world +odom_pose = [3.5, 10.3, 0.] +#odom_pose = [3.5, 11.25, 0.] # Test hallway bend +x0s = [] +reset_conditions = [] + +# NOTE: This is map pose (The order of quaternion also different). +map_state = [#np.array([5.5, 9.2, 0., # Position x, y, z + #1., 0., 0., 0., # Quaternion w, z, (x, y?) + #0., 0., 0., # Linear Velocities + #0., 0., 0.]), # Angular Velocities + np.array([3.5, 9.55, 0., # Position x, y, z + 1., 0., 0., 0., # Quaternion w, z, (x, y?) + 0., 0., 0., # Linear Velocities + 0., 0., 0.]), # Angular Velocities + np.array([3.5, 10.3, 0., # Position x, y, z + 1., 0., 0., 0., # Quaternion w, z, (x, y?) + 0., 0., 0., # Linear Velocities + 0., 0., 0.]), # Angular Velocities + np.array([3.5, 11.2, 0., # Position x, y, z + 1., 0., 0., 0., # Quaternion w, z, (x, y?) + 0., 0., 0., # Linear Velocities + 0., 0., 0.]), # Angular Velocities + #np.array([5.5, 11.5, 0., # Position x, y, z + #1., 0., 0., 0., # Quaternion w, z, (x, y?) + #0., 0., 0., # Linear Velocities + #0., 0., 0.]), # Angular Velocities + ] + +goal_conditions = [#np.array([5.5, 9.2, 0., # Position x, y, z + #1., 0., 0., 0., # Quaternion w, z, (x, y?) + #0., 0., 0., # Linear Velocities + #0., 0., 0.]), # Angular Velocities + np.array([27.5, 12.3, 0., # Position x, y, z + 1., 0., 0., 0., # Quaternion w, z, (x, y?) + 0., 0., 0., # Linear Velocities + 0., 0., 0.]), # Angular Velocities + np.array([27.5, 12.6, 0., # Position x, y, z + 1., 0., 0., 0., # Quaternion w, z, (x, y?) + 0., 0., 0., # Linear Velocities + 0., 0., 0.]), # Angular Velocities + np.array([27.5, 12.9, 0., # Position x, y, z + 1., 0., 0., 0., # Quaternion w, z, (x, y?) + 0., 0., 0., # Linear Velocities + 0., 0., 0.]), # Angular Velocities + #np.array([5.5, 11.5, 0., # Position x, y, z + #1., 0., 0., 0., # Quaternion w, z, (x, y?) + #0., 0., 0., # Linear Velocities + #0., 0., 0.]), # Angular Velocities +] + +# Test Hallway bend +''' +map_state = [np.array([8.5, 11.2, 0., # Position x, y, z + 1., 0., 0., 0., # Quaternion w, z, (x, y?) + 0., 0., 0., # Linear Velocities + 0., 0., 0.]), # Angular Velocities + ] +''' +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': len(map_state), + 'use_mpc': False, +} + +if not os.path.exists(common['data_files_dir']): + os.makedirs(common['data_files_dir']) + +for i in xrange(common['conditions']): + idx_pos = SENSOR_DIMS[MOBILE_POSITION] + idx_ori = SENSOR_DIMS[MOBILE_ORIENTATION] + + state = np.zeros(map_state[i].size) + state[:idx_pos] = map_state[i][:idx_pos] - odom_pose + + # odom state is independent to map state + state[idx_pos:idx_pos+idx_ori] = [0., 0., 0., 1.] + + x0s.append(state) + reset_conditions.append(map_state[i]) + +agent = { + 'type': AgentMPEPCTurtlebot, + 'dt': 0.05, + 'conditions': common['conditions'], + 'T': 150, + 'x0': x0s, + 'use_mpc': common['use_mpc'], + 'M': 10, + 'reset_conditions': reset_conditions, + 'goal_conditions': goal_conditions, + 'sensor_dims': SENSOR_DIMS, + 'state_include': [MOBILE_POSITION, MOBILE_ORIENTATION, \ + MOBILE_VELOCITIES_LINEAR, MOBILE_VELOCITIES_ANGULAR], + 'obs_include': [MOBILE_RANGE_SENSOR, MOBILE_ORIENTATION, \ + MOBILE_VELOCITIES_LINEAR, MOBILE_VELOCITIES_ANGULAR], +} + +algorithm = { + 'type': AlgorithmBADMM, + 'conditions': common['conditions'], + 'iterations': 10, + 'lg_step_schedule': np.array([1e-4, 1e-3, 1e-2, 1e-2]), + 'policy_dual_rate': 0.2, + 'ent_reg_schedule': np.array([1e-3, 1e-3, 1e-2, 1e-1]), + 'fixed_lg_step': 3, + 'kl_step': 5.0, + 'min_step_mult': 0.01, + 'max_step_mult': 1.0, + 'sample_decrease_var': 0.05, + 'sample_increase_var': 0.1, +} + +# algorithm = { +# 'type': AlgorithmPIGPS, +# 'conditions': common['conditions'], +# 'policy_sample_mode': 'replace', +# 'sample_on_policy': True, +# } + +algorithm['init_traj_distr'] = { + 'type': init_pd, + 'init_var': 1.0, + 'pos_gains': 0.0, + 'dQ': SENSOR_DIMS[ACTION], + 'dt': agent['dt'], + 'T': agent['T'], +} + +algorithm['init_mpc'] = { + 'type': init_pd, + 'init_var': 5.0, + 'pos_gains': 0.0, + 'dQ': SENSOR_DIMS[ACTION], + 'dt': agent['dt'], + 'T': agent['M'], +} + + +action_cost = { + 'type': CostAction, + 'wu': np.ones(SENSOR_DIMS[ACTION])*5e-5 +} + +state_cost = { + 'type': CostState, + 'data_types' : { + MOBILE_ORIENTATION: { + 'wp': np.ones(SENSOR_DIMS[MOBILE_ORIENTATION])*100., + 'target_state': np.array([0., 0., 0., 1.]), + }, + MOBILE_VELOCITIES_LINEAR: { + 'wp': np.ones(SENSOR_DIMS[MOBILE_VELOCITIES_LINEAR])*100., + 'target_state': np.array([1.5, 0., 0.]), + }, + MOBILE_VELOCITIES_ANGULAR: { + 'wp': np.ones(SENSOR_DIMS[MOBILE_VELOCITIES_ANGULAR])*2.5, + 'target_state': np.array([0., 0., 0.]), + }, + }, +} + +obstacle_cost = { + 'type': CostObstacle, + 'obstacle_type' : POSITION_NEAREST_OBSTACLE, + 'position_type': MOBILE_POSITION, + 'wp': np.ones(SENSOR_DIMS[MOBILE_POSITION]), + 'd_safe': 1.0 +} + +algorithm['cost'] = { + 'type': CostSum, + 'costs': [action_cost, state_cost, obstacle_cost], + 'weights': [1.0, 1.0, 50.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['traj_opt'] = { +# 'type': TrajOptPI2, +# 'kl_threshold': 2.0, +# 'covariance_damping': 2.0, +# 'min_temperature': 0.001, +# } + +algorithm['policy_opt'] = { + 'type': PolicyOptCaffe, + 'weights_file_prefix': EXP_DIR + 'policy', + 'iterations': 10000, + 'network_arch_params': { + 'n_layers': 2, + 'dim_hidden': [20], + }, +} + +algorithm['policy_prior'] = { + 'type': PolicyPriorGMM, + 'max_clusters': 20, + 'min_samples_per_cluster': 40, + 'max_samples': 20, +} + +config = { + 'iterations': 10, + 'num_samples': 5, + 'common': common, + 'verbose_trials': 0, + 'verbose_policy_trials': 1, + 'agent': agent, + 'gui_on': True, + 'algorithm': algorithm, +} + +common['info'] = generate_experiment_info(config) diff --git a/experiments/turtlebot_mpepc_hallway_pigps_example/Old/hyperparams.py b/experiments/turtlebot_mpepc_hallway_pigps_example/Old/hyperparams.py new file mode 100644 index 000000000..c99b6d9ca --- /dev/null +++ b/experiments/turtlebot_mpepc_hallway_pigps_example/Old/hyperparams.py @@ -0,0 +1,279 @@ +""" Hyperparameters for PR2 trajectory optimization experiment. """ +from __future__ import division + +from datetime import datetime +import os.path + +import numpy as np + +from gps import __file__ as gps_filepath +from gps.agent.ros.agent_mpepc_turtlebot import AgentMPEPCTurtlebot +from gps.algorithm.algorithm_pigps import AlgorithmPIGPS +from gps.algorithm.algorithm_pigps import AlgorithmMDGPS +from gps.algorithm.cost.cost_fk import CostFK +from gps.algorithm.cost.cost_action import CostAction +from gps.algorithm.cost.cost_state import CostState +from gps.algorithm.cost.cost_obstacles import CostObstacle +from gps.algorithm.cost.cost_mpepc import CostMPEPC +from gps.algorithm.cost.cost_sum import CostSum +from gps.algorithm.cost.cost_utils import RAMP_LINEAR, RAMP_FINAL_ONLY +from gps.algorithm.dynamics.dynamics_lr_prior import DynamicsLRPrior +from gps.algorithm.dynamics.dynamics_prior_gmm import DynamicsPriorGMM +from gps.algorithm.policy.policy_prior_gmm import PolicyPriorGMM +from gps.algorithm.traj_opt.traj_opt_pi2 import TrajOptPI2 +from gps.algorithm.policy_opt.policy_opt_caffe import PolicyOptCaffe +from gps.algorithm.policy.lin_gauss_init import init_lqr, init_pd +from gps.gui.target_setup_gui import load_pose_from_npz +from gps.proto.gps_pb2 import MOBILE_POSITION, MOBILE_ORIENTATION, \ + MOBILE_VELOCITIES_LINEAR, MOBILE_VELOCITIES_ANGULAR, ACTION, \ + POSITION_NEAREST_OBSTACLE, POTENTIAL_SCORE, MOBILE_RANGE_SENSOR +from gps.utility.general_utils import get_ee_points +from gps.gui.config import generate_experiment_info + +SENSOR_DIMS = { + MOBILE_POSITION: 3, + MOBILE_ORIENTATION: 4, + MOBILE_VELOCITIES_LINEAR: 3, + MOBILE_VELOCITIES_ANGULAR: 3, + MOBILE_RANGE_SENSOR: 30, + ACTION: 4 +} + +BASE_DIR = '/'.join(str.split(gps_filepath, '/')[:-2]) +EXP_DIR = BASE_DIR + '/../experiments/turtlebot_mpepc_hallway_pigps_example/' + +# NOTE: This is odom pose. +# Default is in one_stacle.world +odom_pose = [3.5, 10.3, 0.] +#odom_pose = [3.5, 11.25, 0.] # Test hallway bend +x0s = [] +reset_conditions = [] + +# NOTE: This is map pose (The order of quaternion also different). +map_state = [#np.array([5.5, 9.2, 0., # Position x, y, z + #1., 0., 0., 0., # Quaternion w, z, (x, y?) + #0., 0., 0., # Linear Velocities + #0., 0., 0.]), # Angular Velocities + np.array([3.5, 9.55, 0., # Position x, y, z + 1., 0., 0., 0., # Quaternion w, z, (x, y?) + 0., 0., 0., # Linear Velocities + 0., 0., 0.]), # Angular Velocities + np.array([3.5, 10.3, 0., # Position x, y, z + 1., 0., 0., 0., # Quaternion w, z, (x, y?) + 0., 0., 0., # Linear Velocities + 0., 0., 0.]), # Angular Velocities + np.array([3.5, 11.2, 0., # Position x, y, z + 1., 0., 0., 0., # Quaternion w, z, (x, y?) + 0., 0., 0., # Linear Velocities + 0., 0., 0.]), # Angular Velocities + #np.array([5.5, 11.5, 0., # Position x, y, z + #1., 0., 0., 0., # Quaternion w, z, (x, y?) + #0., 0., 0., # Linear Velocities + #0., 0., 0.]), # Angular Velocities + ] + +goal_conditions = [#np.array([5.5, 9.2, 0., # Position x, y, z + #1., 0., 0., 0., # Quaternion w, z, (x, y?) + #0., 0., 0., # Linear Velocities + #0., 0., 0.]), # Angular Velocities + np.array([27.5, 12.3, 0., # Position x, y, z + 1., 0., 0., 0., # Quaternion w, z, (x, y?) + 0., 0., 0., # Linear Velocities + 0., 0., 0.]), # Angular Velocities + np.array([27.5, 12.6, 0., # Position x, y, z + 1., 0., 0., 0., # Quaternion w, z, (x, y?) + 0., 0., 0., # Linear Velocities + 0., 0., 0.]), # Angular Velocities + np.array([27.5, 12.9, 0., # Position x, y, z + 1., 0., 0., 0., # Quaternion w, z, (x, y?) + 0., 0., 0., # Linear Velocities + 0., 0., 0.]), # Angular Velocities + #np.array([5.5, 11.5, 0., # Position x, y, z + #1., 0., 0., 0., # Quaternion w, z, (x, y?) + #0., 0., 0., # Linear Velocities + #0., 0., 0.]), # Angular Velocities +] + +# Test Hallway bend +''' +map_state = [np.array([8.5, 11.2, 0., # Position x, y, z + 1., 0., 0., 0., # Quaternion w, z, (x, y?) + 0., 0., 0., # Linear Velocities + 0., 0., 0.]), # Angular Velocities + ] +''' +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': len(map_state), + 'use_mpc': False, +} + +if not os.path.exists(common['data_files_dir']): + os.makedirs(common['data_files_dir']) + +for i in xrange(common['conditions']): + idx_pos = SENSOR_DIMS[MOBILE_POSITION] + idx_ori = SENSOR_DIMS[MOBILE_ORIENTATION] + + state = np.zeros(map_state[i].size) + state[:idx_pos] = map_state[i][:idx_pos] - odom_pose + + # odom state is independent to map state + state[idx_pos:idx_pos+idx_ori] = [0., 0., 0., 1.] + + x0s.append(state) + reset_conditions.append(map_state[i]) + +agent = { + 'type': AgentMPEPCTurtlebot, + 'dt': 0.05, + 'conditions': common['conditions'], + 'T': 150, + 'x0': x0s, + 'use_mpc': common['use_mpc'], + 'M': 10, + 'reset_conditions': reset_conditions, + 'goal_conditions': goal_conditions, + 'sensor_dims': SENSOR_DIMS, + 'state_include': [MOBILE_POSITION, MOBILE_ORIENTATION, \ + MOBILE_VELOCITIES_LINEAR, MOBILE_VELOCITIES_ANGULAR], + 'obs_include': [MOBILE_RANGE_SENSOR, MOBILE_ORIENTATION, \ + MOBILE_VELOCITIES_LINEAR, MOBILE_VELOCITIES_ANGULAR], +} + +algorithm = { + 'type': AlgorithmPIGPS, + 'conditions': common['conditions'], + 'policy_sample_mode': 'replace', + 'sample_on_policy': True, +} + +algorithm['init_traj_distr'] = { + 'type': init_pd, + 'init_var': 1.0, + 'pos_gains': 0.0, + 'dQ': SENSOR_DIMS[ACTION], + 'dt': agent['dt'], + 'T': agent['T'], +} + +algorithm['init_mpc'] = { + 'type': init_pd, + 'init_var': 5.0, + 'pos_gains': 0.0, + 'dQ': SENSOR_DIMS[ACTION], + 'dt': agent['dt'], + 'T': agent['M'], +} + + +action_cost = { + 'type': CostAction, + 'wu': np.ones(SENSOR_DIMS[ACTION])*5e-5 +} + +state_cost = { + 'type': CostState, + 'data_types' : { + MOBILE_ORIENTATION: { + 'wp': np.ones(SENSOR_DIMS[MOBILE_ORIENTATION])*100., + 'target_state': np.array([0., 0., 0., 1.]), + }, + MOBILE_VELOCITIES_LINEAR: { + 'wp': np.ones(SENSOR_DIMS[MOBILE_VELOCITIES_LINEAR])*10., + 'target_state': np.array([1.0, 0., 0.]), + }, + MOBILE_VELOCITIES_ANGULAR: { + 'wp': np.ones(SENSOR_DIMS[MOBILE_VELOCITIES_ANGULAR])*2.5, + 'target_state': np.array([0., 0., 0.]), + }, + }, +} +# +# obstacle_cost = { +# 'type': CostObstacle, +# 'obstacle_type' : POSITION_NEAREST_OBSTACLE, +# 'position_type': MOBILE_POSITION, +# 'wp': np.ones(SENSOR_DIMS[MOBILE_POSITION]), +# 'd_safe': 1.0 +# } +# +# algorithm['cost'] = { +# 'type': CostSum, +# 'costs': [action_cost, state_cost, obstacle_cost], +# 'weights': [1.0, 1.0, 50.0], +# } + +mpepc_cost = { + 'type': CostMPEPC, + 'obstacle_type' : POSITION_NEAREST_OBSTACLE, + 'position_type': MOBILE_POSITION, + 'potential_type' : POTENTIAL_SCORE, + 'wp': np.ones(SENSOR_DIMS[MOBILE_POSITION]), + 'wp_col' : 10.0, + 'wp_nf' : 1.0, + 'sigma_square': 0.4, +} + +algorithm['cost'] = { + 'type': CostSum, + 'costs': [action_cost, state_cost, mpepc_cost], + 'weights': [1.0, 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['traj_opt'] = { + 'type': TrajOptPI2, + 'kl_threshold': 2.0, + 'covariance_damping': 2.0, + 'min_temperature': 0.001, +} + +algorithm['policy_opt'] = { + 'type': PolicyOptCaffe, + 'weights_file_prefix': EXP_DIR + 'policy', + 'iterations': 10000, + 'network_arch_params': { + 'n_layers': 2, + 'dim_hidden': [20], + }, +} + +algorithm['policy_prior'] = { + 'type': PolicyPriorGMM, + 'max_clusters': 20, + 'min_samples_per_cluster': 40, + 'max_samples': 20, +} + +config = { + 'iterations': 30, + 'num_samples': 20, + 'common': common, + 'verbose_trials': 0, + 'verbose_policy_trials': 1, + 'agent': agent, + 'gui_on': True, + 'algorithm': algorithm, +} + +common['info'] = generate_experiment_info(config) diff --git a/experiments/turtlebot_mpepc_hallway_pigps_example/hyperparams.py b/experiments/turtlebot_mpepc_hallway_pigps_example/hyperparams.py new file mode 100644 index 000000000..d6ad651a8 --- /dev/null +++ b/experiments/turtlebot_mpepc_hallway_pigps_example/hyperparams.py @@ -0,0 +1,259 @@ +""" Hyperparameters for PR2 trajectory optimization experiment. """ +from __future__ import division + +from datetime import datetime +import os.path + +import numpy as np + +from gps import __file__ as gps_filepath +from gps.agent.ros.agent_mpepc_turtlebot import AgentMPEPCTurtlebot +from gps.algorithm.algorithm_pigps import AlgorithmPIGPS +from gps.algorithm.algorithm_pigps import AlgorithmMDGPS +from gps.algorithm.cost.cost_fk import CostFK +from gps.algorithm.cost.cost_action import CostAction +from gps.algorithm.cost.cost_state import CostState +from gps.algorithm.cost.cost_obstacles import CostObstacle +from gps.algorithm.cost.cost_mpepc import CostMPEPC +from gps.algorithm.cost.cost_sum import CostSum +from gps.algorithm.cost.cost_utils import RAMP_LINEAR, RAMP_FINAL_ONLY +from gps.algorithm.dynamics.dynamics_lr_prior import DynamicsLRPrior +from gps.algorithm.dynamics.dynamics_prior_gmm import DynamicsPriorGMM +from gps.algorithm.policy.policy_prior_gmm import PolicyPriorGMM +from gps.algorithm.traj_opt.traj_opt_pi2 import TrajOptPI2 +from gps.algorithm.policy_opt.policy_opt_caffe import PolicyOptCaffe +from gps.algorithm.policy.lin_gauss_init import init_lqr, init_pd +from gps.gui.target_setup_gui import load_pose_from_npz +from gps.proto.gps_pb2 import MOBILE_POSITION, MOBILE_ORIENTATION, \ + MOBILE_VELOCITIES_LINEAR, MOBILE_VELOCITIES_ANGULAR, ACTION, \ + POSITION_NEAREST_OBSTACLE, POTENTIAL_SCORE, MOBILE_RANGE_SENSOR +from gps.utility.general_utils import get_ee_points +from gps.gui.config import generate_experiment_info + +SENSOR_DIMS = { + MOBILE_POSITION: 3, + MOBILE_ORIENTATION: 4, + MOBILE_VELOCITIES_LINEAR: 3, + MOBILE_VELOCITIES_ANGULAR: 3, + MOBILE_RANGE_SENSOR: 30, + ACTION: 4 +} + +BASE_DIR = '/'.join(str.split(gps_filepath, '/')[:-2]) +EXP_DIR = BASE_DIR + '/../experiments/turtlebot_mpepc_hallway_pigps_example/' + +# NOTE: This is odom pose. +# Default is in one_stacle.world +odom_pose = [3.5, 10.3, 0.] +#odom_pose = [3.5, 11.25, 0.] # Test hallway bend +x0s = [] +reset_conditions = [] + +# NOTE: This is map pose (The order of quaternion also different). +map_state = [ + # np.array([9.5, 9.55, 0., # Position x, y, z + # 1., 0., 0., 0., # Quaternion w, z, (x, y?) + # 0., 0., 0., # Linear Velocities + # 0., 0., 0.]), # Angular Velocities + np.array([9.5, 10.3, 0., # Position x, y, z + 1., 0., 0., 0., # Quaternion w, z, (x, y?) + 0., 0., 0., # Linear Velocities + 0., 0., 0.]), # Angular Velocities + # np.array([9.5, 11.2, 0., # Position x, y, z + # 1., 0., 0., 0., # Quaternion w, z, (x, y?) + # 0., 0., 0., # Linear Velocities + # 0., 0., 0.]), # Angular Velocities + ] + +goal_conditions = [ + # np.array([27.5, 12.3, 0., # Position x, y, z + # 1., 0., 0., 0., # Quaternion w, z, (x, y?) + # 0., 0., 0., # Linear Velocities + # 0., 0., 0.]), # Angular Velocities + np.array([27.5, 12.6, 0., # Position x, y, z + 1., 0., 0., 0., # Quaternion w, z, (x, y?) + 0., 0., 0., # Linear Velocities + 0., 0., 0.]), # Angular Velocities + # np.array([27.5, 12.9, 0., # Position x, y, z + # 1., 0., 0., 0., # Quaternion w, z, (x, y?) + # 0., 0., 0., # Linear Velocities + # 0., 0., 0.]), # Angular Velocities +] + +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': len(map_state), + 'use_mpc': False, +} + +if not os.path.exists(common['data_files_dir']): + os.makedirs(common['data_files_dir']) + +for i in xrange(common['conditions']): + idx_pos = SENSOR_DIMS[MOBILE_POSITION] + idx_ori = SENSOR_DIMS[MOBILE_ORIENTATION] + + state = np.zeros(map_state[i].size) + state[:idx_pos] = map_state[i][:idx_pos] - odom_pose + + # odom state is independent to map state + state[idx_pos:idx_pos+idx_ori] = [0., 0., 0., 1.] + + x0s.append(state) + reset_conditions.append(map_state[i]) + +agent = { + 'type': AgentMPEPCTurtlebot, + 'dt': 0.05, + 'conditions': common['conditions'], + 'T': 100, + 'x0': x0s, + 'use_mpc': common['use_mpc'], + 'M': 10, + 'reset_conditions': reset_conditions, + 'goal_conditions': goal_conditions, + 'sensor_dims': SENSOR_DIMS, + 'state_include': [MOBILE_POSITION, MOBILE_ORIENTATION, \ + MOBILE_VELOCITIES_LINEAR, MOBILE_VELOCITIES_ANGULAR], + # 'obs_include': [MOBILE_POSITION, MOBILE_ORIENTATION, \ + # MOBILE_VELOCITIES_LINEAR, MOBILE_VELOCITIES_ANGULAR], + 'obs_include': [MOBILE_RANGE_SENSOR, MOBILE_ORIENTATION, \ + MOBILE_VELOCITIES_LINEAR, MOBILE_VELOCITIES_ANGULAR], +} + +algorithm = { + 'type': AlgorithmPIGPS, + 'conditions': common['conditions'], + 'policy_sample_mode': 'replace', + 'sample_on_policy': True, +} + +algorithm['init_traj_distr'] = { + 'type': init_pd, + 'init_var': 1.0, + 'pos_gains': 0.0, + 'dQ': SENSOR_DIMS[ACTION], + 'dt': agent['dt'], + 'T': agent['T'], +} + +algorithm['init_mpc'] = { + 'type': init_pd, + 'init_var': 5.0, + 'pos_gains': 0.0, + 'dQ': SENSOR_DIMS[ACTION], + 'dt': agent['dt'], + 'T': agent['M'], +} + + +action_cost = { + 'type': CostAction, + 'wu': np.ones(SENSOR_DIMS[ACTION])*5e-5 +} + +state_cost = { + 'type': CostState, + 'data_types' : { + # MOBILE_ORIENTATION: { + # 'wp': np.ones(SENSOR_DIMS[MOBILE_ORIENTATION])*100., + # 'target_state': np.array([0., 0., 0., 1.]), + # }, + MOBILE_VELOCITIES_LINEAR: { + 'wp': np.ones(SENSOR_DIMS[MOBILE_VELOCITIES_LINEAR])*100., + 'target_state': np.array([1.0, 0., 0.]), + }, + MOBILE_VELOCITIES_ANGULAR: { + 'wp': np.ones(SENSOR_DIMS[MOBILE_VELOCITIES_ANGULAR])*2.5, + 'target_state': np.array([0., 0., 0.]), + }, + }, +} + +# obstacle_cost = { +# 'type': CostObstacle, +# 'obstacle_type' : POSITION_NEAREST_OBSTACLE, +# 'position_type': MOBILE_POSITION, +# 'wp': np.ones(SENSOR_DIMS[MOBILE_POSITION]), +# 'd_safe': 1.0 +# } +# +# algorithm['cost'] = { +# 'type': CostSum, +# 'costs': [action_cost, state_cost, obstacle_cost], +# 'weights': [1.0, 1.0, 50.0], +# } + +mpepc_cost = { + 'type': CostMPEPC, + 'obstacle_type' : POSITION_NEAREST_OBSTACLE, + 'position_type': MOBILE_POSITION, + 'potential_type' : POTENTIAL_SCORE, + 'wp': np.ones(SENSOR_DIMS[MOBILE_POSITION]), + 'wp_col' : 30.0, + 'wp_nf' : 100.0, + 'sigma_square': 0.2, +} + +algorithm['cost'] = { + 'type': CostSum, + 'costs': [action_cost, state_cost, mpepc_cost], + 'weights': [1.0, 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['traj_opt'] = { + 'type': TrajOptPI2, + 'kl_threshold': 2.0, + 'covariance_damping': 1.0, + 'min_temperature': 0.001, +} + +algorithm['policy_opt'] = { + 'type': PolicyOptCaffe, + 'weights_file_prefix': EXP_DIR + 'policy', + 'iterations': 10000, + 'network_arch_params': { + 'n_layers': 2, + 'dim_hidden': [20], + }, +} + +algorithm['policy_prior'] = { + 'type': PolicyPriorGMM, + 'max_clusters': 20, + 'min_samples_per_cluster': 40, + 'max_samples': 20, +} + +config = { + 'iterations': 30, + 'num_samples': 20, + 'common': common, + 'verbose_trials': 0, + 'verbose_policy_trials': 1, + 'agent': agent, + 'gui_on': True, + 'algorithm': algorithm, +} + +common['info'] = generate_experiment_info(config) diff --git a/python/gps/agent/agent.py b/python/gps/agent/agent.py index 6afc76583..fdf5d5391 100755 --- a/python/gps/agent/agent.py +++ b/python/gps/agent/agent.py @@ -270,3 +270,7 @@ def unpack_data_x(self, existing_mat, data_types, axes=None): index[axes[i]] = slice(self._x_data_idx[data_types[i]][0], self._x_data_idx[data_types[i]][-1] + 1) return existing_mat[index] + + def publish_plan(self, state, mpc=False): + # Publish plan given + pass \ No newline at end of file diff --git a/python/gps/agent/box2d/agent_box2d.py b/python/gps/agent/box2d/agent_box2d.py index af77a6ff2..f294e6226 100644 --- a/python/gps/agent/box2d/agent_box2d.py +++ b/python/gps/agent/box2d/agent_box2d.py @@ -21,6 +21,7 @@ def __init__(self, hyperparams): self._setup_conditions() self._setup_world(self._hyperparams["world"], + self._hyperparams["world_info"], self._hyperparams["target_state"], self._hyperparams["render"]) @@ -34,16 +35,16 @@ def _setup_conditions(self): 'noisy_body_idx', 'noisy_body_var'): self._hyperparams[field] = setup(self._hyperparams[field], conds) - def _setup_world(self, world, target, render): + def _setup_world(self, world, world_info, target, render): """ Helper method for handling setup of the Box2D world. """ self.x0 = self._hyperparams["x0"] - self._worlds = [world(self.x0[i], target, render) + self._worlds = [world(self.x0[i], world_info, target, render) for i in range(self._hyperparams['conditions'])] - def sample(self, policy, condition, verbose=False, save=True, noisy=True): + def sample(self, policy, condition, reset=True, verbose=False, save=True, noisy=True): """ Runs a trial and constructs a new sample containing information about the trial. @@ -55,8 +56,9 @@ def sample(self, policy, condition, verbose=False, save=True, noisy=True): save (boolean): Whether or not to store the trial into the samples. noisy (boolean): Whether or not to use noise during sampling. """ - self._worlds[condition].run() - self._worlds[condition].reset_world() + if reset: + self._worlds[condition].run() + self._worlds[condition].reset_world() b2d_X = self._worlds[condition].get_state() new_sample = self._init_sample(b2d_X) U = np.zeros([self.T, self.dU]) diff --git a/python/gps/agent/box2d/arm_world.py b/python/gps/agent/box2d/arm_world.py index 2ffe13a9c..527883602 100644 --- a/python/gps/agent/box2d/arm_world.py +++ b/python/gps/agent/box2d/arm_world.py @@ -8,7 +8,7 @@ class ArmWorld(Framework): """ This class defines the 2 Link Arm and its environment.""" name = "2 Link Arm" - def __init__(self, x0, target, render): + def __init__(self, x0, world_info, target, render): self.render = render if self.render: super(ArmWorld, self).__init__() diff --git a/python/gps/agent/box2d/point_mass_world.py b/python/gps/agent/box2d/point_mass_world.py index c9a7dc80d..30c16f8b4 100644 --- a/python/gps/agent/box2d/point_mass_world.py +++ b/python/gps/agent/box2d/point_mass_world.py @@ -9,7 +9,7 @@ class PointMassWorld(Framework): """ This class defines the point mass and its environment.""" name = "PointMass" - def __init__(self, x0, target, render): + def __init__(self, x0, world_info, target, render): self.render = render if self.render: super(PointMassWorld, self).__init__() @@ -97,4 +97,4 @@ def get_state(self): state = {END_EFFECTOR_POINTS: np.append(np.array(self.body.position), [0]), END_EFFECTOR_POINT_VELOCITIES: np.append(np.array(self.body.linearVelocity), [0])} - return state + return state \ No newline at end of file diff --git a/python/gps/agent/box2d/point_mass_world_obstacle.py b/python/gps/agent/box2d/point_mass_world_obstacle.py new file mode 100644 index 000000000..a99fcb898 --- /dev/null +++ b/python/gps/agent/box2d/point_mass_world_obstacle.py @@ -0,0 +1,189 @@ +""" This file defines an environment for the Box2D PointMass simulator. """ +import numpy as np +import Box2D as b2 +from framework import Framework + +from gps.agent.box2d.settings import fwSettings +from gps.proto.gps_pb2 import END_EFFECTOR_POINTS, END_EFFECTOR_POINT_VELOCITIES, POSITION_NEAREST_OBSTACLE + +class PointMassWorldObstacle(Framework): + """ This class defines the point mass and its environment.""" + name = "PointMass" + def __init__(self, x0, world_info, target, render): + self.render = render + if self.render: + super(PointMassWorldObstacle, self).__init__() + else: + self.world = b2.b2World(gravity=(0, -10), doSleep=True) + self.world.gravity = (0.0, 0.0) + self.initial_position = (x0[0], x0[1]) + self.initial_angle = b2.b2_pi + self.initial_linear_velocity = (x0[2], x0[3]) + self.initial_angular_velocity = 0 + + ground = self.world.CreateBody(position=(0, 20)) + ground.CreateEdgeChain( + [(-20, -20), + (-20, 20), + (20, 20), + (20, -20), + (-20, -20)] + ) + + xf1 = b2.b2Transform() + xf1.angle = 0.3524 * b2.b2_pi + xf1.position = b2.b2Mul(xf1.R, (1.0, 0.0)) + + xf2 = b2.b2Transform() + xf2.angle = -0.3524 * b2.b2_pi + xf2.position = b2.b2Mul(xf2.R, (-1.0, 0.0)) + + """ + self.body_shape = [b2.b2PolygonShape(vertices=[xf1*(-1, 0), + xf1*(1, 0), xf1*(0, .5)]), + b2.b2PolygonShape(vertices=[xf2*(-1, 0), + xf2*(1, 0), xf2*(0, .5)])] + """ + self.body_shape = [b2.b2PolygonShape(box=(1,1))] + self.body = self.world.CreateDynamicBody( + position=self.initial_position, + angle=self.initial_angle, + linearVelocity=self.initial_linear_velocity, + angularVelocity=self.initial_angular_velocity, + angularDamping=5, + linearDamping=0.1, + shapes=self.body_shape, + shapeFixture=b2.b2FixtureDef(density=1.0), + ) + self.initial_pos = self.world.CreateStaticBody( + position=self.initial_position, + angle=self.initial_angle, + shapes=self.body_shape, + ) + self.target = self.world.CreateStaticBody( + position=target[:2], + angle=self.initial_angle, + shapes=[b2.b2PolygonShape(vertices=[xf1*(-1, 0), xf1*(1, 0), + xf1*(0, .5)]), + b2.b2PolygonShape(vertices=[xf2*(-1, 0), xf2*(1, 0), + xf2*(0, .5)])], + ) + + self.obstacle_post = [] + self.obstacle_shape = [] + self.obstacle = [] + obstacles = world_info['obstacles'] + self.n_obs = len(obstacles) + for i in range(self.n_obs): + self.obstacle_post.append(obstacles[i][:2]) + self.obstacle_shape.append([b2.b2PolygonShape(box=tuple(obstacles[i][2:]))]) + self.obstacle.append(self.world.CreateStaticBody( + position=self.obstacle_post[i], + angle=self.initial_angle, + shapes=self.obstacle_shape[i] + )) + + self.initial_pos.active = False + self.target.active = False + + def run(self): + """Initiates the first time step + """ + if self.render: + super(PointMassWorldObstacle, self).run() + else: + self.run_next(None) + + def run_next(self, action): + """Moves forward in time one step. Calls the renderer if applicable.""" + if self.render: + super(PointMassWorldObstacle, self).run_next(action) + """ + hist = self.world.CreateStaticBody( + position=self.body.position, + angle=self.body.angle, + shapes=self.body_shape + ) + hist.active = False + """ + #self.world.DestroyBody(hist) + else: + if action is not None: + self.body.linearVelocity = (action[0], action[1]) + self.world.Step(1.0 / fwSettings.hz, fwSettings.velocityIterations, + fwSettings.positionIterations) + + def Step(self, settings, action): + """Called upon every step. """ + self.body.linearVelocity = (action[0], action[1]) + + super(PointMassWorldObstacle, self).Step(settings) + + def reset_world(self): + """ This resets the world to its initial state""" + self.world.ClearForces() + self.body.position = self.initial_position + self.body.angle = self.initial_angle + self.body.angularVelocity = self.initial_angular_velocity + self.body.linearVelocity = self.initial_linear_velocity + # Reset obstacle also + for i in range(self.n_obs): + self.obstacle.append(self.world.CreateStaticBody( + position=self.obstacle_post[i], + angle=self.initial_angle, + shapes=self.obstacle_shape[i] + )) + + def get_nearest_dist_obs(self): + dist = None + position = None + + for i in range(self.n_obs): + distanceInput = b2.b2DistanceInput(); + distanceInput.transformA = self.body.transform + distanceInput.transformB = self.obstacle[i].transform + # TODO: Show how multi polygon to one shape instance + distanceInput.proxyA = b2.b2DistanceProxy(self.body_shape[0]) + distanceInput.proxyB = b2.b2DistanceProxy(self.obstacle_shape[i][0]) + distanceInput.useRadii = True + + distanceOutput = b2.b2Distance(distanceInput) + + if dist is None or distanceOutput.distance < dist: + position = distanceOutput.pointB + dist = distanceOutput.distance + + + """ + # euclidian_dist == distanceOutput.distance + euclidian_dist = np.sqrt((self.body.position[0] - distanceOutput.pointB[0]) ** 2 + \ + (self.body.position[1] - distanceOutput.pointB[1]) ** 2) + + # gradient of euclidian_dist. For simplific, I power 2 the distance. + grad = np.append(np.array(self.body.position), [0]) + + return np.array([0.5 * euclidian_dist ** 2]) + """ + + return np.append(np.array(position), [0]) + + def drawPose(self, poseArray, position): + hist = self.world.CreateStaticBody( + position=position, + angle=self.body.angle, + shapes=self.body_shape + ) + hist.active = False + poseArray.append(hist) + + def clearPose(self, poseArray): + for hist in poseArray: + self.world.DestroyBody(hist) + + def get_state(self): + """ This retrieves the state of the point mass""" + state = {END_EFFECTOR_POINTS: np.append(np.array(self.body.position), [0]), + END_EFFECTOR_POINT_VELOCITIES: np.append(np.array(self.body.linearVelocity), [0]), + POSITION_NEAREST_OBSTACLE: self.get_nearest_dist_obs()} + + return state diff --git a/python/gps/agent/config.py b/python/gps/agent/config.py index 9c2e18a6a..afa318839 100755 --- a/python/gps/agent/config.py +++ b/python/gps/agent/config.py @@ -18,6 +18,7 @@ 'smooth_noise': True, 'smooth_noise_var': 2.0, 'smooth_noise_renormalize': True, + 'use_mpc': False } @@ -54,6 +55,30 @@ 300.0, 0.0, 2.0, 2.0 ]), } + + AGENT_TURTLEBOT = { + #TODO: Check is needed to change these name ? + 'trial_command_topic': 'gps_controller_trial_command', + 'reset_command_topic': 'gps_controller_navigation_command', + 'data_request_topic': 'gps_controller_data_request', + 'sample_result_topic': 'gps_controller_report', + 'trial_timeout': 20, # Give this many seconds for a trial. + 'reset_conditions': [], # Defines reset modes + positions for + # trial and auxiliary arms. + 'frequency': 20, + } + + AGENT_MPEPEC_TURTLEBOT = { + #TODO: Check is needed to change these name ? + 'trial_command_topic': 'gps_controller_trial_command', + 'reset_command_topic': 'gps_controller_navigation_command', + 'data_request_topic': 'gps_controller_data_request', + 'sample_result_topic': 'gps_controller_report', + 'trial_timeout': 20, # Give this many seconds for a trial. + 'reset_conditions': [], # Defines reset modes + positions for + # trial and auxiliary arms. + 'frequency': 20, + } except ImportError as e: AGENT_ROS = {} LOGGER.debug('No ROS enabled: %s', e) @@ -74,4 +99,5 @@ AGENT_BOX2D = { 'render': True, + 'world_info': None } diff --git a/python/gps/agent/ros/agent_mpepc_turtlebot.py b/python/gps/agent/ros/agent_mpepc_turtlebot.py new file mode 100644 index 000000000..78faaaf28 --- /dev/null +++ b/python/gps/agent/ros/agent_mpepc_turtlebot.py @@ -0,0 +1,60 @@ +''' +Created on Mar 3, 2017 + +@author: thobotics +''' +import copy +import time +import numpy as np + +import rospy + +from gps.agent.ros.agent_turtlebot import AgentTurtlebot +from gps.agent.agent_utils import generate_noise, setup +from gps.agent.config import AGENT_MPEPEC_TURTLEBOT +from geometry_msgs.msg import PoseArray, Pose, PoseStamped +from __builtin__ import raw_input + + +class AgentMPEPCTurtlebot(AgentTurtlebot): + """ + All communication between the algorithms and ROS is done through + this class. + """ + def __init__(self, hyperparams, init_node=True): + """ + Initialize agent. + Args: + hyperparams: Dictionary of hyperparameters. + init_node: Whether or not to initialize a new ROS node. + """ + config = copy.deepcopy(AGENT_MPEPEC_TURTLEBOT) + config.update(hyperparams) + AgentTurtlebot.__init__(self, config) + + def _init_pubs_and_subs(self): + AgentTurtlebot._init_pubs_and_subs(self) + self._global_goal = rospy.Publisher("global_goal", PoseStamped) + + def reset(self, condition): + """ + Reset the agent for a particular experiment condition. + Args: + condition: An index into hyperparams['goal_conditions']. + """ + AgentTurtlebot.reset(self, condition) + + # Publish global plan + pose = PoseStamped() + pose.header.stamp = rospy.Time.now() + pose.header.frame_id = "/map" + + condition_data = self._hyperparams['goal_conditions'][condition] + pose.pose.position.x = condition_data[0] + pose.pose.position.y = condition_data[1] + pose.pose.position.z = condition_data[2] + pose.pose.orientation.x = condition_data[3] + pose.pose.orientation.y = condition_data[4] + pose.pose.orientation.z = condition_data[5] + pose.pose.orientation.w = condition_data[6] + self._global_goal.publish(pose) diff --git a/python/gps/agent/ros/agent_ros.py b/python/gps/agent/ros/agent_ros.py index eaadca0fa..31dd002fd 100644 --- a/python/gps/agent/ros/agent_ros.py +++ b/python/gps/agent/ros/agent_ros.py @@ -137,7 +137,7 @@ def reset(self, condition): condition_data[AUXILIARY_ARM]['data']) time.sleep(2.0) # useful for the real robot, so it stops completely - def sample(self, policy, condition, verbose=True, save=True, noisy=True): + def sample(self, policy, condition, reset=True, verbose=True, save=True, noisy=True): """ Reset and execute a policy and collect a sample. Args: @@ -153,7 +153,8 @@ def sample(self, policy, condition, verbose=True, save=True, noisy=True): if isinstance(policy, TfPolicy): self._init_tf(policy.dU) - self.reset(condition) + if reset: + self.reset(condition) # Generate noise. if noisy: noise = generate_noise(self.T, self.dU, self._hyperparams) diff --git a/python/gps/agent/ros/agent_turtlebot.py b/python/gps/agent/ros/agent_turtlebot.py new file mode 100644 index 000000000..71880bb67 --- /dev/null +++ b/python/gps/agent/ros/agent_turtlebot.py @@ -0,0 +1,168 @@ +''' +Created on Mar 3, 2017 + +@author: thobotics +''' +import copy +import time +import numpy as np + +import rospy + +from gps.agent.agent import Agent +from gps.agent.agent_utils import generate_noise, setup +from gps.agent.config import AGENT_TURTLEBOT +from gps.agent.ros.ros_utils import ServiceEmulator, msg_to_sample, \ + policy_to_msg, tf_policy_to_action_msg, tf_obs_msg_to_numpy +from gps.proto.gps_pb2 import TRIAL_ARM, AUXILIARY_ARM +from gps_agent_pkg.msg import TrialCommand, SampleResult, NavigationCommand, \ + DataRequest +from geometry_msgs.msg import PoseArray, Pose +from __builtin__ import raw_input + + +class AgentTurtlebot(Agent): + """ + All communication between the algorithms and ROS is done through + this class. + """ + def __init__(self, hyperparams, init_node=True): + """ + Initialize agent. + Args: + hyperparams: Dictionary of hyperparameters. + init_node: Whether or not to initialize a new ROS node. + """ + config = copy.deepcopy(AGENT_TURTLEBOT) + config.update(hyperparams) + Agent.__init__(self, config) + if init_node: + rospy.init_node('gps_agent_turtlebot_node') + self._init_pubs_and_subs() + self._seq_id = 0 # Used for setting seq in ROS commands. + + self.x0 = self._hyperparams['x0'] + + r = rospy.Rate(1) + r.sleep() + + #self.use_tf = False + #self.observations_stale = True + + def _init_pubs_and_subs(self): + self._trial_service = ServiceEmulator( + self._hyperparams['trial_command_topic'], TrialCommand, + self._hyperparams['sample_result_topic'], SampleResult + ) + self._reset_service = ServiceEmulator( + self._hyperparams['reset_command_topic'], NavigationCommand, + self._hyperparams['sample_result_topic'], SampleResult + ) + self._data_service = ServiceEmulator( + self._hyperparams['data_request_topic'], DataRequest, + self._hyperparams['sample_result_topic'], SampleResult + ) + self._offline_plan = rospy.Publisher("ofline_plan", PoseArray) + self._mpc_plan = rospy.Publisher("mpc_plan", PoseArray) + + def _get_next_seq_id(self): + self._seq_id = (self._seq_id + 1) % (2 ** 32) + return self._seq_id + + def get_data(self): + """ + Request for the most recent value for data/sensor readings. + Returns entire sample report (all available data) in sample. + """ + request = DataRequest() + request.id = self._get_next_seq_id() + request.arm = TRIAL_ARM + request.stamp = rospy.get_rostime() + result_msg = self._data_service.publish_and_wait(request) + # TODO - Make IDs match, assert that they match elsewhere here. + sample = msg_to_sample(result_msg, self) + return sample + + def reset(self, condition): + """ + Reset the agent for a particular experiment condition. + Args: + condition: An index into hyperparams['reset_conditions']. + """ + condition_data = self._hyperparams['reset_conditions'][condition] + reset_command = NavigationCommand() + reset_command.position = condition_data[:2] + reset_command.quaternion = condition_data[3:7] + timeout = self._hyperparams['trial_timeout'] + reset_command.id = self._get_next_seq_id() + self._reset_service.publish_and_wait(reset_command, timeout=timeout) + time.sleep(2.0) # useful for the real robot, so it stops completely + + def sample(self, policy, condition, reset=True, verbose=True, save=True, noisy=True): + """ + Reset and execute a policy and collect a sample. + Args: + policy: A Policy object. + condition: Which condition setup to run. + verbose: Unused for this agent. + save: Whether or not to store the trial into the samples. + noisy: Whether or not to use noise during sampling. + Returns: + sample: A Sample object. + """ + if reset: + self.reset(condition) + # Generate noise. + if noisy: + noise = generate_noise(self.T, self.dU, self._hyperparams) + else: + noise = np.zeros((self.T, self.dU)) + # noise = noise*0.01 + + # Execute trial. + trial_command = TrialCommand() + trial_command.id = self._get_next_seq_id() + trial_command.controller = policy_to_msg(policy, noise) + trial_command.T = self.T + trial_command.id = self._get_next_seq_id() + trial_command.frequency = self._hyperparams['frequency'] + # ee_points and ee_points_tgt is uneccesary for mobile robot + trial_command.ee_points = [] + trial_command.ee_points_tgt = [] + trial_command.state_datatypes = self._hyperparams['state_include'] + trial_command.obs_datatypes = self._hyperparams['obs_include'] + + sample_msg = self._trial_service.publish_and_wait( + trial_command, timeout=self._hyperparams['trial_timeout'] + ) + + sample = msg_to_sample(sample_msg, self) + if save: + self._samples[condition].append(sample) + return sample + + def publish_plan(self, state, mpc=False): + T, _ = state.shape + + poseArray = PoseArray() + poseArray.header.stamp = rospy.Time.now() + poseArray.header.frame_id = "/odom" + + for t in range(T): + pose = Pose() + pose.position.x = state[t,0] + pose.position.y = state[t,1] + pose.position.z = state[t,2] + + pose.orientation.x = state[t,3] + pose.orientation.y = state[t,4] + pose.orientation.z = state[t,5] + pose.orientation.w = state[t,6] + + poseArray.poses.append(pose) + + # Publish plan given + if mpc: + self._mpc_plan.publish(poseArray) + else: + self._offline_plan.publish(poseArray) \ No newline at end of file diff --git a/python/gps/algorithm/algorithm.py b/python/gps/algorithm/algorithm.py index d09954f09..b3f7954a6 100755 --- a/python/gps/algorithm/algorithm.py +++ b/python/gps/algorithm/algorithm.py @@ -10,7 +10,7 @@ from gps.algorithm.config import ALG from gps.algorithm.algorithm_utils import IterationData, TrajectoryInfo from gps.utility.general_utils import extract_condition - +from gps.algorithm.traj_opt.mpc_traj_opt import MpcTrajOpt LOGGER = logging.getLogger(__name__) @@ -77,6 +77,15 @@ def __init__(self, hyperparams): for _ in range(self.M) ] self.base_kl_step = self._hyperparams['kl_step'] + + self.mpc = [] # For initialize + + def init_mpc(self, num_samples, hyperparams): + for m in range(self.M): + self.mpc.append([]) + + for i in range(num_samples): + self.mpc[m].append(MpcTrajOpt(hyperparams, m)) @abc.abstractmethod def iteration(self, sample_list): diff --git a/python/gps/algorithm/algorithm_badmm.py b/python/gps/algorithm/algorithm_badmm.py index ce40e7d96..9e32444bd 100644 --- a/python/gps/algorithm/algorithm_badmm.py +++ b/python/gps/algorithm/algorithm_badmm.py @@ -124,17 +124,31 @@ def _update_policy(self, inner_itr): wt = np.zeros((N, T)) # Get time-indexed actions. for t in range(T): - # Compute actions along this trajectory. - prc[:, t, :, :] = np.tile(traj.inv_pol_covar[t, :, :], - [N, 1, 1]) - for i in range(N): - mu[i, t, :] = \ - (traj.K[t, :, :].dot(X[i, t, :]) + traj.k[t, :]) - \ - np.linalg.solve( - prc[i, t, :, :] / pol_info.pol_wt[t], - pol_info.lambda_K[t, :, :].dot(X[i, t, :]) + \ - pol_info.lambda_k[t, :] - ) + if not self.mpc: # is empty + # Compute actions along this trajectory. + prc[:, t, :, :] = np.tile(traj.inv_pol_covar[t, :, :], + [N, 1, 1]) + for i in range(N): + mu[i, t, :] = \ + (traj.K[t, :, :].dot(X[i, t, :]) + traj.k[t, :]) - \ + np.linalg.solve( + prc[i, t, :, :] / pol_info.pol_wt[t], + pol_info.lambda_K[t, :, :].dot(X[i, t, :]) + \ + pol_info.lambda_k[t, :] + ) + else: + for i in range(N): + mpc = self.mpc[m][i] + n, t_mpc = mpc.convert_t_traj(t) + mpc_pol = mpc.mpc_pol[n] + prc[i, t, :, :] = mpc_pol.inv_pol_covar[t_mpc, :, :] + mu[i, t, :] = \ + (mpc_pol.K[t_mpc, :, :].dot(X[i, t, :]) + mpc_pol.k[t_mpc, :]) - \ + np.linalg.solve( + prc[i, t, :, :] / pol_info.pol_wt[t], + pol_info.lambda_K[t, :, :].dot(X[i, t, :]) + \ + pol_info.lambda_k[t, :] + ) wt[:, t].fill(pol_info.pol_wt[t]) tgt_mu = np.concatenate((tgt_mu, mu)) tgt_prc = np.concatenate((tgt_prc, prc)) diff --git a/python/gps/algorithm/config.py b/python/gps/algorithm/config.py index a247f3fdc..d1ca688b9 100644 --- a/python/gps/algorithm/config.py +++ b/python/gps/algorithm/config.py @@ -28,7 +28,9 @@ # Whether or not to sample with neural net policy (only for badmm/mdgps). 'sample_on_policy': False, # Inidicates if the algorithm requires fitting of the dynamics. - 'fit_dynamics': True, + 'fit_dynamics': True, + # Inidicates use mpc for sample and as a policy trainners. + 'use_mpc': False, } diff --git a/python/gps/algorithm/cost/config.py b/python/gps/algorithm/cost/config.py index 82e299482..f857921fd 100644 --- a/python/gps/algorithm/cost/config.py +++ b/python/gps/algorithm/cost/config.py @@ -33,6 +33,31 @@ }, } +# CostObstacle +COST_OBSTACLE = { + 'ramp_option': RAMP_CONSTANT, # How target cost ramps over time. + 'l1': 0.0, + 'l2': 1.0, + 'alpha': 1e-2, + 'wp_final_multiplier': 1.0, # Weight multiplier on final time step. + 'wp': None, + 'obstacle_type': None, + 'position_type': None, + 'd_safe': 0.4, +} + +COST_MPEPC = { + 'ramp_option': RAMP_CONSTANT, # How target cost ramps over time. + 'wp_final_multiplier': 1.0, # Weight multiplier on final time step. + 'wp': None, + 'wp_col': None, + 'wp_nf': None, + 'sigma_square': 0.1, + 'obstacle_type': None, + 'position_type': None, + 'potential_type': None, +} + # CostBinaryRegion COST_BINARY_REGION = { 'ramp_option': RAMP_CONSTANT, # How target cost ramps over time. diff --git a/python/gps/algorithm/cost/cost_mpepc.py b/python/gps/algorithm/cost/cost_mpepc.py new file mode 100644 index 000000000..6ae4d8114 --- /dev/null +++ b/python/gps/algorithm/cost/cost_mpepc.py @@ -0,0 +1,71 @@ +import copy + +import numpy as np +import math +from gps.algorithm.cost.config import COST_MPEPC +from gps.algorithm.cost.cost import Cost +from gps.algorithm.cost.cost_utils import evalhinglel2loss, get_ramp_multiplier + +class CostMPEPC(Cost): + """ Computes hingle l2 loss for a given closest obstacle position. """ + def __init__(self, hyperparams): + config = copy.deepcopy(COST_MPEPC) + config.update(hyperparams) + Cost.__init__(self, config) + + def eval(self, sample): + """ + Evaluate cost function and derivatives on a sample. + Args: + sample: A single sample + """ + T = sample.T + dX = sample.dX + dU = sample.dU + + # Initialize terms. + l = np.zeros(T) + lu = np.zeros((T, dU)) + lx = np.zeros((T, dX)) + luu = np.zeros((T, dU, dU)) + lxx = np.zeros((T, dX, dX)) + lux = np.zeros((T, dU, dX)) + + wp = self._hyperparams['wp'] + + obs = sample.get(self._hyperparams['obstacle_type']) + x = sample.get(self._hyperparams['position_type']) + pot = sample.get(self._hyperparams['potential_type']) + + wpm = get_ramp_multiplier( + self._hyperparams['ramp_option'], T, + wp_final_multiplier=self._hyperparams['wp_final_multiplier'] + ) + wp = wp * np.expand_dims(wpm, axis=-1) + + # Compute state penalty. + obs_dist = x - obs + + sigma_square = self._hyperparams['sigma_square'] + p_collision = np.exp(- np.sum(obs_dist ** 2, axis=1) / sigma_square) + l_collision = p_collision * self._hyperparams['wp_col'] * wpm + + pot_prev = np.zeros_like(pot[:, 0]) + pot_prev[0] = 0 # Penalty for first + pot_prev[1:] = pot[:-1,0] + p_survivability = 1 - p_collision + l_progress = p_survivability * (pot[:, 0] - pot_prev) * self._hyperparams['wp_nf'] * wpm + + l = l_collision + l_progress + + # Evaluate penalty term. + # l, ls, lss = evalhinglel2loss( + # wp, dist, self._hyperparams['d_safe'], self._hyperparams['l2'], + # ) + # + # # Add to current terms. + # sample.agent.pack_data_x(lx, ls, data_types=[data_type]) + # sample.agent.pack_data_x(lxx, lss, + # data_types=[data_type, data_type]) + + return l, lx, lu, lxx, luu, lux diff --git a/python/gps/algorithm/cost/cost_obstacles.py b/python/gps/algorithm/cost/cost_obstacles.py new file mode 100644 index 000000000..a09c2abac --- /dev/null +++ b/python/gps/algorithm/cost/cost_obstacles.py @@ -0,0 +1,58 @@ +import copy + +import numpy as np + +from gps.algorithm.cost.config import COST_OBSTACLE +from gps.algorithm.cost.cost import Cost +from gps.algorithm.cost.cost_utils import evalhinglel2loss, get_ramp_multiplier + +class CostObstacle(Cost): + """ Computes hingle l2 loss for a given closest obstacle position. """ + def __init__(self, hyperparams): + config = copy.deepcopy(COST_OBSTACLE) + config.update(hyperparams) + Cost.__init__(self, config) + + def eval(self, sample): + """ + Evaluate cost function and derivatives on a sample. + Args: + sample: A single sample + """ + T = sample.T + dX = sample.dX + dU = sample.dU + + # Initialize terms. + l = np.zeros(T) + lu = np.zeros((T, dU)) + lx = np.zeros((T, dX)) + luu = np.zeros((T, dU, dU)) + lxx = np.zeros((T, dX, dX)) + lux = np.zeros((T, dU, dX)) + + data_type = self._hyperparams['position_type'] + + wp = self._hyperparams['wp'] + obs = sample.get(self._hyperparams['obstacle_type']) + x = sample.get(data_type) + + wpm = get_ramp_multiplier( + self._hyperparams['ramp_option'], T, + wp_final_multiplier=self._hyperparams['wp_final_multiplier'] + ) + wp = wp * np.expand_dims(wpm, axis=-1) + # Compute state penalty. + dist = x - obs + + # Evaluate penalty term. + l, ls, lss = evalhinglel2loss( + wp, dist, self._hyperparams['d_safe'], self._hyperparams['l2'], + ) + + # Add to current terms. + sample.agent.pack_data_x(lx, ls, data_types=[data_type]) + sample.agent.pack_data_x(lxx, lss, + data_types=[data_type, data_type]) + + return l, lx, lu, lxx, luu, lux diff --git a/python/gps/algorithm/cost/cost_utils.py b/python/gps/algorithm/cost/cost_utils.py index 00f15bcdb..527c0f16c 100644 --- a/python/gps/algorithm/cost/cost_utils.py +++ b/python/gps/algorithm/cost/cost_utils.py @@ -28,6 +28,41 @@ def get_ramp_multiplier(ramp_option, T, wp_final_multiplier=1.0): wpm[-1] *= wp_final_multiplier return wpm +def evalhinglel2loss(wp, d, dsafe, l2): + """ + loss = max(0, dsafe - 0.5 * l2 * d^2) + Args: + wp: T, matrix with weights for each dimension and time step. + d: T x Dx states to evaluate norm on. + dsafe: is safe distance + """ + # Get trajectory length. + T, Dx = d.shape + + sqrtwp = np.sqrt(wp) + dsclsq = d * sqrtwp + dscl = d * wp + + # loss + l2d = 0.5 * np.sum(dsclsq ** 2, axis=1) * l2 + l = np.maximum(0, dsafe - l2d).reshape(T,) + + # gradient - subgradient + # = -dscl*l2 if dsafe > d + # = 0 otherwise + bin_l = l > 0 + lx = -1.0 * np.tile(bin_l, [Dx, 1]).transpose() * (dscl * l2) + + # hessian + # = -l2 for diagonal element, if dsafe > d + # = 0 otherwise + idx = np.where(bin_l == 1)[0] + hes = np.zeros([T, Dx, Dx]) + hes[idx, :, :] = np.eye(Dx) + lxx = -1.0 * l2 * (np.expand_dims(wp, axis=2) * hes) + + return l, lx, lxx + def evall1l2term(wp, d, Jd, Jdd, l1, l2, alpha): """ diff --git a/python/gps/algorithm/policy/lin_gauss_policy.py b/python/gps/algorithm/policy/lin_gauss_policy.py index 127278227..bb2f27e0a 100644 --- a/python/gps/algorithm/policy/lin_gauss_policy.py +++ b/python/gps/algorithm/policy/lin_gauss_policy.py @@ -1,8 +1,10 @@ """ This file defines the linear Gaussian policy class. """ import numpy as np +import logging from gps.algorithm.policy.policy import Policy from gps.utility.general_utils import check_shape +LOGGER = logging.getLogger(__name__) class LinearGaussianPolicy(Policy): @@ -50,13 +52,20 @@ def fold_k(self, noise): Returns: k: A T x dU bias vector. """ + #mean_noise = np.zeros_like(noise) k = np.zeros_like(self.k) for i in range(self.T): scaled_noise = self.chol_pol_covar[i].T.dot(noise[i]) k[i] = scaled_noise + self.k[i] + #mean_noise[i] = np.abs(scaled_noise) + + #print "Mean ", np.mean(mean_noise, axis=0) + #print "Chol covar: " + #print self.chol_pol_covar + return k - def nans_like(self): + def nans_like(self, zeros=False): """ Returns: A new linear Gaussian policy object with the same dimensions @@ -67,9 +76,11 @@ def nans_like(self): np.zeros_like(self.pol_covar), np.zeros_like(self.chol_pol_covar), np.zeros_like(self.inv_pol_covar) ) - policy.K.fill(np.nan) - policy.k.fill(np.nan) - policy.pol_covar.fill(np.nan) - policy.chol_pol_covar.fill(np.nan) - policy.inv_pol_covar.fill(np.nan) + if zeros == False: + policy.K.fill(np.nan) + policy.k.fill(np.nan) + policy.pol_covar.fill(np.nan) + policy.chol_pol_covar.fill(np.nan) + policy.inv_pol_covar.fill(np.nan) + return policy diff --git a/python/gps/algorithm/traj_opt/mpc_traj_opt.py b/python/gps/algorithm/traj_opt/mpc_traj_opt.py new file mode 100644 index 000000000..f0666235b --- /dev/null +++ b/python/gps/algorithm/traj_opt/mpc_traj_opt.py @@ -0,0 +1,366 @@ +""" This file defines code for MPC GPS based trajectory optimization. + +References: + [1] Tianhao Zhang, Gregory Kahn, Sergey Levine, Pieter Abbeel. Learning Deep Control Policies + for Autonomous Aerial Vehicles with MPC-Guided Policy Search. ICRA 2016. + """ + +import os +import os.path +import sys +import logging +import numpy as np +import scipy as sp +from copy import deepcopy +from math import ceil +from numpy.linalg import LinAlgError +from gps.algorithm.config import ALG +from gps.utility.general_utils import extract_condition + +LOGGER = logging.getLogger(__name__) + + +class MpcTrajOpt(object): + def __init__(self, hyperparams, cond): + config = deepcopy(ALG) + config.update(hyperparams) + self._hyperparams = config + + agent = self._hyperparams['agent'] + self.T = self._hyperparams['T'] = agent.T + self.M = self._hyperparams['M'] = config['init_mpc']['T'] + self.dU = self._hyperparams['dU'] = agent.dU + self.dX = self._hyperparams['dX'] = agent.dX + del self._hyperparams['agent'] # Don't want to pickle this + + self.N = int(ceil(self.T/(self.M-1.))) + + # It will update wwith different X_t in update function + # Note that is different from M in last n-th MPC + self.T_mpc = self.M + + # Setup policy + init_mpc = config['init_mpc'] + init_mpc['x0'] = agent.x0 + init_mpc['dX'] = agent.dX + init_mpc['dU'] = agent.dU + + self.mpc_pol = [] + for n in range(self.N): + init_mpc = extract_condition( + config['init_mpc'], cond + ) + self.mpc_pol.append(init_mpc['type'](init_mpc)) + + def convert_t_traj(self, t_traj): + return t_traj / (self.M - 1), t_traj % (self.M - 1) + + def update(self, n, X_t, prior, traj_distr, traj_info, cur_t, pol_info=None): + self.T = traj_distr.T + dX = traj_distr.dX + dU = traj_distr.dU + + # Make a copy + trajinfo = deepcopy(traj_info) + trajinfo.x0mu = X_t + trajinfo.x0sigma = 1e-6*np.eye(dX) + + if cur_t+self.M > self.T: + X_ref = prior[cur_t:,:dX] + else: + X_ref = prior[cur_t:cur_t+self.M,:dX] + + # Reset T_mpc + self.T_mpc = X_ref.shape[0] + + mu, sigma = self.forward(traj_distr, trajinfo, cur_t) + new_mpc = self.backward(self.mpc_pol[n], traj_distr, trajinfo, X_ref, mu, sigma, cur_t, pol_info) + + # Forward to get plan state + new_mu, new_sigma = self.forward(new_mpc, trajinfo, cur_t, mpc=True) + # Store mpc + self.mpc_pol[n] = new_mpc + + return new_mpc, new_mu + + def forward(self, traj_distr, traj_info, cur_t, mpc=False): + """ + Perform LQR forward pass. Computes state-action marginals from + dynamics and policy. + Args: + traj_distr: A linear Gaussian policy object. + traj_info: A TrajectoryInfo object. + Returns: + mu: A T x dX mean action vector. + sigma: A T x dX x dX covariance matrix. + """ + # Compute state-action marginals from specified conditional + # parameters and current traj_info. + T = self.M + dU = traj_distr.dU + dX = traj_distr.dX + + # Constants. + idx_x = slice(dX) + + # Allocate space. + sigma = np.zeros((T, dX+dU, dX+dU)) + mu = np.zeros((T, dX+dU)) + + # Pull out dynamics. + Fm = traj_info.dynamics.Fm + fv = traj_info.dynamics.fv + dyn_covar = traj_info.dynamics.dyn_covar + + # Set initial covariance (initial mu is always zero). + sigma[0, idx_x, idx_x] = traj_info.x0sigma + mu[0, idx_x] = traj_info.x0mu + + for t in range(T): + # t trajectory distribution + t_dyn = cur_t+t + # t dynamics + if mpc: + t_traj = t + else: + t_traj = cur_t+t + + if t_dyn > self.T-1: + break + sigma[t, :, :] = np.vstack([ + np.hstack([ + sigma[t, idx_x, idx_x], + sigma[t, idx_x, idx_x].dot(traj_distr.K[t_traj, :, :].T) + ]), + np.hstack([ + traj_distr.K[t_traj, :, :].dot(sigma[t, idx_x, idx_x]), + traj_distr.K[t_traj, :, :].dot(sigma[t, idx_x, idx_x]).dot( + traj_distr.K[t_traj, :, :].T + ) + traj_distr.pol_covar[t_traj, :, :] + ]) + ]) + mu[t, :] = np.hstack([ + mu[t, idx_x], + traj_distr.K[t_traj, :, :].dot(mu[t, idx_x]) + traj_distr.k[t_traj, :] + ]) + if t < T - 1: + sigma[t+1, idx_x, idx_x] = \ + Fm[t_dyn, :, :].dot(sigma[t, :, :]).dot(Fm[t_dyn, :, :].T) + \ + dyn_covar[t_dyn, :, :] + mu[t+1, idx_x] = Fm[t_dyn, :, :].dot(mu[t, :]) + fv[t_dyn, :] + return mu, sigma + + def backward(self, prev_mpc_traj_distr, traj_distr, traj_info, x0, mu, sigma, cur_t, pol_info): + """ + Perform LQR backward pass. This computes a new linear Gaussian + policy object. + Args: + prev_mpc_traj_distr: previous MPC + traj_distr: A linear Gaussian policy object from + previous iteration. + traj_info: A TrajectoryInfo object. + x0: State independent from action generate by forward + pass from offline trajectory distribution. + mu, sigma: Parameter of forward pass independent from + action, start from current real state. + cur_t: current time of agent + Returns: + traj_distr: A new MPC linear Gaussian policy. + """ + # Constants. + T = self.T_mpc + dU = prev_mpc_traj_distr.dU + dX = prev_mpc_traj_distr.dX + + mpc_traj_distr = prev_mpc_traj_distr.nans_like(zeros=True) + + """ + # TODO: Check BADMM need this? + # Store pol_wt if necessary + if type(algorithm) == AlgorithmBADMM: + pol_wt = algorithm.cur[m].pol_info.pol_wt + """ + + idx_x = slice(dX) + idx_u = slice(dX, dX+dU) + + # Pull out dynamics. + Fm = traj_info.dynamics.Fm + fv = traj_info.dynamics.fv + + # Non-SPD correction terms. + eta = 0 + del_ = 1e-4 + eta0 = eta + + # Run dynamic programming. + fail = True + while fail: + fail = False # Flip to true on non-symmetric PD. + + # Allocate. + Vxx = np.zeros((T, dX, dX)) + Vx = np.zeros((T, dX)) + + fCm, fcv = self.compute_costs(traj_distr, x0, mu, sigma, cur_t, pol_info) + + # Compute state-action-state function at each time step. + for t in range(T - 1, -1, -1): + t_traj = cur_t+t + # Add in the cost. + Qtt = fCm[t, :, :] # (X+U) x (X+U) + Qt = fcv[t, :] # (X+U) x 1 + + # Add in the value function from the next time step. + if t < T - 1: + """ + # TODO: Check BADMM need this? + if type(algorithm) == AlgorithmBADMM: + multiplier = (pol_wt[t+1] + eta)/(pol_wt[t] + eta) + else: + multiplier = 1.0 + """ + multiplier = 1.0 + Qtt = Qtt + multiplier * \ + Fm[t_traj, :, :].T.dot(Vxx[t+1, :, :]).dot(Fm[t_traj, :, :]) + Qt = Qt + multiplier * \ + Fm[t_traj, :, :].T.dot(Vx[t+1, :] + + Vxx[t+1, :, :].dot(fv[t_traj, :])) + + # Symmetrize quadratic component. + Qtt = 0.5 * (Qtt + Qtt.T) + + # Regularization to make sure Quu is PD + Qtt[idx_u, idx_u] += eta*np.eye(dU) + + # Compute Cholesky decomposition of Q function action + # component. + try: + U = sp.linalg.cholesky(Qtt[idx_u, idx_u]) + L = U.T + except LinAlgError as e: + # Error thrown when Qtt[idx_u, idx_u] is not + # symmetric positive definite. + #LOGGER.debug('MPC LinAlgError: %s', e) + fail = True + break + + # Store conditional covariance, inverse, and Cholesky. + mpc_traj_distr.inv_pol_covar[t, :, :] = Qtt[idx_u, idx_u] + mpc_traj_distr.pol_covar[t, :, :] = sp.linalg.solve_triangular( + U, sp.linalg.solve_triangular(L, np.eye(dU), lower=True) + ) + mpc_traj_distr.chol_pol_covar[t, :, :] = sp.linalg.cholesky( + mpc_traj_distr.pol_covar[t, :, :] + ) + + # Compute mean terms. + mpc_traj_distr.k[t, :] = -sp.linalg.solve_triangular( + U, sp.linalg.solve_triangular(L, Qt[idx_u], lower=True) + ) + mpc_traj_distr.K[t, :, :] = -sp.linalg.solve_triangular( + U, sp.linalg.solve_triangular(L, Qtt[idx_u, idx_x], + lower=True) + ) + + # Compute value function. + Vxx[t, :, :] = Qtt[idx_x, idx_x] + \ + Qtt[idx_x, idx_u].dot(mpc_traj_distr.K[t, :, :]) + Vx[t, :] = Qt[idx_x] + Qtt[idx_x, idx_u].dot(mpc_traj_distr.k[t, :]) + Vxx[t, :, :] = 0.5 * (Vxx[t, :, :] + Vxx[t, :, :].T) + + # Increment eta on non-SPD Q-function. + if fail: + old_eta = eta + eta = eta0 + del_ + LOGGER.debug('Increasing eta: %f -> %f', old_eta, eta) + del_ *= 2 # Increase del_ exponentially on failure. + if eta >= 1e16: + if np.any(np.isnan(Fm)) or np.any(np.isnan(fv)): + raise ValueError('NaNs encountered in dynamics!') + raise ValueError('Failed to find PD solution even for very \ + large eta (check that dynamics and cost are \ + reasonably well conditioned)!') + return mpc_traj_distr + + def _eval_cost(self, x0, mu, sigma): + T = self.T_mpc + dX = self.dX + dU = self.dU + + fCm = np.zeros([T, dX+dU, dX+dU]) + fcv = np.zeros([T, dX+dU]) + + idx_x = slice(dX) + idx_u = slice(dX, dX+dU) + + # Cost = -logp(x_t'|x_t) + # Gradient = inv(Sigma)*(x0 - mu) + # Hessian = inv(Sigma) + for t in range(T - 1, -1, -1): + inv_sigma = np.linalg.inv(sigma[t,idx_x,idx_x]) + fcv[t, idx_x] = inv_sigma.dot(x0[t] - mu[t,idx_x]) # Gradient + fCm[t, idx_x, idx_x] = inv_sigma # Hessian + + #yhat = np.c_[x0, u0] + yhat = np.c_[x0] + rdiff = -yhat + rdiff_expand = np.expand_dims(rdiff, axis=2) + cv_update = np.sum(fCm[:,idx_x,idx_x] * rdiff_expand, axis=1) + fcv[:,idx_x] += cv_update + + return fCm, fcv + + def compute_costs(self, traj_distr, x0, mu, sigma, cur_t, pol_info): + T = self.T_mpc + dX = self.dX + dU = self.dU + fCm, fcv = self._eval_cost(x0, mu, sigma) + + if pol_info is not None: + # Modify policy action via Lagrange multiplier. + fcv[:, dX:] -= pol_info.lambda_k[cur_t:cur_t+T] + fCm[:, dX:, :dX] -= pol_info.lambda_K[cur_t:cur_t+T] + fCm[:, :dX, dX:] -= np.transpose(pol_info.lambda_K[cur_t:cur_t+T], [0, 2, 1]) + + # NOTE: Copy from policy_opt_caffe, and it work. + # If using raw wt policy cost diverge. + wts = pol_info.pol_wt * (float(T) / np.sum(pol_info.pol_wt)) + # Allow weights to be at most twice the robust median. + mn = np.median(wts[(wts > 1e-2).nonzero()]) + for t in range(T): + wts[t] = min(wts[t], 2 * mn) + # Robust median should be around one. + wts /= mn + + # Add in the trajectory divergence term. + for t in range(T - 1, -1, -1): + t_traj = cur_t+t + if pol_info is not None: + # Policy KL-divergence terms. + inv_pol_covar = np.linalg.solve( + pol_info.chol_pol_S[t_traj, :, :], + np.linalg.solve(pol_info.chol_pol_S[t_traj, :, :].T, np.eye(dU)) + ) + K, k = pol_info.pol_K[t_traj, :, :], pol_info.pol_k[t_traj, :] + wt = wts[t_traj] + else: + K, k = traj_distr.K[t_traj, :, :], traj_distr.k[t_traj, :] + inv_pol_covar = traj_distr.inv_pol_covar[t_traj, :, :] + wt = 1.0 # No weight + + fCm[t, :, :] += wt * np.vstack([ + np.hstack([ + K.T.dot(inv_pol_covar).dot(K), + -K.T.dot(inv_pol_covar) + ]), + np.hstack([ + -inv_pol_covar.dot(K), inv_pol_covar + ]) + ]) + fcv[t, :] += wt * np.hstack([ + K.T.dot(inv_pol_covar).dot(k), + -inv_pol_covar.dot(k) + ]) + + return fCm, fcv \ No newline at end of file diff --git a/python/gps/gps_main.py b/python/gps/gps_main.py index 5eae6b2fd..79ca62dc2 100755 --- a/python/gps/gps_main.py +++ b/python/gps/gps_main.py @@ -19,6 +19,12 @@ from gps.gui.gps_training_gui import GPSTrainingGUI from gps.utility.data_logger import DataLogger from gps.sample.sample_list import SampleList +from gps.algorithm.traj_opt.mpc_traj_opt import MpcTrajOpt +from gps.algorithm.algorithm_traj_opt import AlgorithmTrajOpt +from gps.sample.sample import Sample +from math import ceil +import numpy as np +from copy import deepcopy class GPSMain(object): @@ -50,6 +56,17 @@ def __init__(self, config, quit_on_end=False): config['algorithm']['agent'] = self.agent self.algorithm = config['algorithm']['type'](config['algorithm']) + + self.use_mpc = False + if 'use_mpc' in config['common'] and config['common']['use_mpc']: + self.use_mpc = True + config['agent']['T'] = config['agent']['M'] + self.mpc_agent = config['agent']['type'](config['agent']) + + # Algorithm __init__ deleted it + config['algorithm']['agent'] = self.agent + + self.algorithm.init_mpc(config['num_samples'], config['algorithm']) def run(self, itr_load=None): """ @@ -183,10 +200,8 @@ def _take_sample(self, itr, cond, i): 'Sampling: iteration %d, condition %d, sample %d.' % (itr, cond, i) ) - self.agent.sample( - pol, cond, - verbose=(i < self._hyperparams['verbose_trials']) - ) + + self._roll_out(pol, itr, cond, i) if self.gui.mode == 'request' and self.gui.request == 'fail': redo = True @@ -194,12 +209,70 @@ def _take_sample(self, itr, cond, i): self.agent.delete_last_sample(cond) else: redo = False + else: + self._roll_out(pol, itr, cond, i) + + def _roll_out(self, pol, itr, cond, i): + if self.use_mpc and itr > 0: + T = self.agent.T + M = self.mpc_agent.T + N = int(ceil(T/(M-1.))) + X_t = self.agent.x0[cond] + + # Only forward pass one time per cond, + # because this same for all sample + if i == 0: + # Note: At this time algorithm.prev = algorithm.cur, + # and prev.traj_info already have x0mu, x0sigma. + self.off_prior, _ = self.algorithm.traj_opt.forward(pol, self.algorithm.prev[cond].traj_info) + self.agent.publish_plan(self.off_prior) + + if type(self.algorithm) == AlgorithmTrajOpt: + pol_info = None + else: + pol_info = self.algorithm.cur[cond].pol_info + + for n in range(N): + # Note: M-1 because action[M] = [0,0]. + t_traj = n*(M-1) + reset = True if(n == 0) else False + + mpc_pol, mpc_state = self.algorithm.mpc[cond][i].update( + n, X_t, self.off_prior, pol, + self.algorithm.cur[cond].traj_info, t_traj, pol_info + ) + self.agent.publish_plan(mpc_state, True) + new_sample = self.mpc_agent.sample( + mpc_pol, cond, + reset=reset, noisy=True, + verbose=(i < self._hyperparams['verbose_trials']) + ) + X_t = new_sample.get_X(t=M-1) + + """ + Merge sample for optimize offline trajectory distribution + """ + full_sample = Sample(self.agent) + sample_lists = self.mpc_agent.get_samples(cond) + keys = sample_lists[0]._data.keys() + t = 0 + for sample in sample_lists: + for m in range(sample.T-1): + for sensor in keys: + full_sample.set(sensor, sample.get(sensor, m), t) + t = t+1 + if t+1 > T: + break + + self.agent._samples[cond].append(full_sample) + # Clear agent samples. + self.mpc_agent.clear_samples() else: self.agent.sample( pol, cond, verbose=(i < self._hyperparams['verbose_trials']) ) - + def _take_iteration(self, itr, sample_lists): """ Take an iteration of the algorithm. diff --git a/python/gps/gui/gps_training_gui.py b/python/gps/gui/gps_training_gui.py index 33e7cdecc..20ac208df 100644 --- a/python/gps/gui/gps_training_gui.py +++ b/python/gps/gui/gps_training_gui.py @@ -18,6 +18,7 @@ import threading import numpy as np +import matplotlib import matplotlib.pyplot as plt import matplotlib.gridspec as gridspec diff --git a/python/tests/test_turtlebot/turtlebot.py b/python/tests/test_turtlebot/turtlebot.py new file mode 100644 index 000000000..75ce60438 --- /dev/null +++ b/python/tests/test_turtlebot/turtlebot.py @@ -0,0 +1,128 @@ +#import matplotlib as mpl +#mpl.use('Qt4Agg') + +import os +import os.path +import sys +import numpy as np +import imp +import Box2D as b2 +from copy import deepcopy +# Add gps/python to path so that imports work. +gps_path = os.path.abspath(os.path.join(os.path.dirname(__file__), '../..', '')) +sys.path.append(gps_path) + +from gps.agent.ros.agent_turtlebot import AgentTurtlebot +from gps.gui.gps_training_gui import GPSTrainingGUI +from gps.utility.data_logger import DataLogger +from gps.sample.sample_list import SampleList +from gps.proto.gps_pb2 import END_EFFECTOR_POINTS, END_EFFECTOR_POINT_VELOCITIES,\ +MOBILE_POSITION, MOBILE_ORIENTATION, POSITION_NEAREST_OBSTACLE, ACTION +from gps.sample.sample import Sample +from gps.algorithm.cost.cost_obstacles import CostObstacle +from gps.algorithm.cost.cost_state import CostState +from gps.algorithm.cost.cost_sum import CostSum +from gps.utility.data_logger import DataLogger +from gps.agent.agent_utils import generate_noise, setup +from gps.agent.config import AGENT +from gps.algorithm.cost.cost_utils import evalhinglel2loss, evall1l2term, evallogl2term +from scipy.stats import multivariate_normal +from numpy.linalg import LinAlgError +from gps.algorithm.traj_opt.mpc_traj_opt import MpcTrajOpt +from gps.algorithm.policy.lin_gauss_init import init_pd +from math import ceil +import scipy as sp +import time + +config = None + +def loadExperiment(exp_name): + from gps import __file__ as gps_filepath + gps_filepath = os.path.abspath(gps_filepath) + gps_dir = '/'.join(str.split(gps_filepath, '/')[:-3]) + '/' + exp_dir = gps_dir + 'experiments/' + exp_name + '/' + hyperparams_file = exp_dir + 'hyperparams.py' + + print hyperparams_file + + if not os.path.exists(hyperparams_file): + sys.exit("Experiment '%s' does not exist.\nDid you create '%s'?" % + (exp_name, hyperparams_file)) + + hyperparams = imp.load_source('hyperparams', hyperparams_file) + + return hyperparams + +def runTest(itr_load): + data_files_dir = config['common']['data_files_dir'] + data_logger = DataLogger() + + algorithm_file = data_files_dir + 'algorithm_itr_%02d.pkl' % itr_load + algorithm = data_logger.unpickle(algorithm_file) + if algorithm is None: + print("Error: cannot find '%s.'" % algorithm_file) + os._exit(1) # called instead of sys.exit(), since this is in a thread + + #pol = algorithm.cur[0].traj_distr + pol = algorithm.policy_opt.policy + agent_hyperparams = deepcopy(AGENT) + agent_hyperparams.update(config['agent']) + cost_obstacle = CostObstacle(config['algorithm']['cost']['costs'][2]) + cost_state = CostState(config['algorithm']['cost']['costs'][1]) + + x0s = agent_hyperparams["x0"] + for cond in range(len(x0s)): + T = agent_hyperparams['T'] + dX = x0s[cond].shape[0] + dU = agent_hyperparams['sensor_dims'][ACTION] + + agent_hyperparams['render'] = True + agent = config['agent']['type'](agent_hyperparams) + time.sleep(1) # Time for init node + + ''' + while True: + sample = agent.get_data() + raw_input("Get data") + ''' + # Sample using offline trajectory distribution. + for i in range(config['num_samples']): + sample = agent.sample(pol, cond, noisy = False) + cost_sum = CostSum(config['algorithm']['cost']) + cost_obs = cost_obstacle.eval(sample)[0] + cost_sta = cost_state.eval(sample)[0] + total_cost = np.sum(cost_sum.eval(sample)[0]) + weights = config['algorithm']['cost']['weights'] + print "Total cost: ", total_cost, + print "Cost state: ", np.sum(weights[1]*cost_sta), + print "Cost obstacle: ", np.sum(weights[2]*cost_obs) + + ''' + l, lx, lu, lxx, luu, lux = cost_obstacle.eval(sample) + sl, slx, slu, slxx, sluu, slux = cost_state.eval(sample) + + for t in range(T): + state = sample.get(MOBILE_POSITION, t) + obs = sample.get(POSITION_NEAREST_OBSTACLE, t) + dist = np.sqrt(np.sum((state - obs)**2)) + print state, obs, dist, 0.5*dist**2, l[t], sl[t] + #print sl[t] + #print lx[t] + ''' + +def main(): + print 'running ros' + #exp_name = "turtlebot_example" + #exp_name = "turtlebot_badmm_example" + exp_name = "turtlebot_hallway_badmm_example" + hyperparams = loadExperiment(exp_name) + global config + config = hyperparams.config + + #runExperiment() + argv = sys.argv + runTest(int(argv[1])) + print "pass test" + +if __name__ == '__main__': + main() diff --git a/src/gps_agent_pkg/.gitignore b/src/gps_agent_pkg/.gitignore index 1f5d4eb2e..f11893ee7 100644 --- a/src/gps_agent_pkg/.gitignore +++ b/src/gps_agent_pkg/.gitignore @@ -2,3 +2,6 @@ src/gps_agent_pkg/__init__.py src/gps_agent_pkg/msg/* +catkin +catkin_generated +devel diff --git a/src/gps_agent_pkg/CMakeLists.txt b/src/gps_agent_pkg/CMakeLists.txt index c669f7c9f..511a66bad 100644 --- a/src/gps_agent_pkg/CMakeLists.txt +++ b/src/gps_agent_pkg/CMakeLists.txt @@ -54,8 +54,14 @@ set(DDP_FILES src/robotplugin.cpp src/trialcontroller.cpp src/encodersensor.cpp src/encoderfilter.cpp - src/rostopicsensor.cpp - src/util.cpp) + src/rostopicsensor.cpp + src/util.cpp + src/mobilerobot.cpp + src/mpepcmobilerobot.cpp + src/mobilerobotsensor.cpp + src/navcontroller.cpp + src/mpepccontrollaw.cpp +) # Include Caffe if (USE_CAFFE) @@ -98,6 +104,16 @@ rosbuild_add_library(gps_agent_lib ${DDP_FILES}) #rosbuild_add_executable(point_head src/point_head.cpp) #rosbuild_add_executable(torso src/torso.cpp) +# ADDED BY THOBOTICS +rosbuild_add_executable(mobilerobot_node src/mobilerobot_node.cpp) +target_link_libraries(mobilerobot_node gps_agent_lib) +rosbuild_add_executable(mpepcmobilerobot_node src/mpepcmobilerobot_node.cpp) +target_link_libraries(mpepcmobilerobot_node gps_agent_lib) +rosbuild_add_executable(globalplanner_node src/globalplanner_node.cpp) +target_link_libraries(globalplanner_node gps_agent_lib) +rosbuild_add_executable(controllaw_node src/controllaw_node.cpp) +target_link_libraries(controllaw_node gps_agent_lib) + # Include Caffe in controller if (USE_CAFFE) target_link_libraries(gps_agent_lib caffe protobuf) diff --git a/src/gps_agent_pkg/include/gps_agent_pkg/mobilerobot.h b/src/gps_agent_pkg/include/gps_agent_pkg/mobilerobot.h new file mode 100644 index 000000000..3b9414780 --- /dev/null +++ b/src/gps_agent_pkg/include/gps_agent_pkg/mobilerobot.h @@ -0,0 +1,95 @@ +/* + * mobilerobot.h + * + * Created on: Mar 5, 2017 + * Author: thobotics + */ + +#ifndef GPS_AGENT_PKG_INCLUDE_GPS_AGENT_PKG_MOBILEROBOT_H_ +#define GPS_AGENT_PKG_INCLUDE_GPS_AGENT_PKG_MOBILEROBOT_H_ + + +#include +#include +#include +#include +#include +#include +#include +#include + +#include "gps_agent_pkg/NavigationCommand.h" +#include "gps_agent_pkg/TrialCommand.h" +#include "gps_agent_pkg/SampleResult.h" +#include "gps_agent_pkg/DataRequest.h" +#include "gps_agent_pkg/sensor.h" +#include "gps_agent_pkg/controller.h" +#include "gps_agent_pkg/navcontroller.h" +#include "gps_agent_pkg/robotplugin.h" +#include "gps/proto/gps.pb.h" + +// Convenience defines. +#define ros_publisher_ptr(X) boost::scoped_ptr > +#define MAX_TRIAL_LENGTH 2000 + +namespace gps_control +{ + +class MobileRobot: public RobotPlugin { +public: + // Constructor (this should do nothing). + MobileRobot(); + // Destructor. + virtual ~MobileRobot(); + + // Init all things needed + void init(ros::NodeHandle& n); + // This is the main update function called by the realtime thread when the controller is running. + void update(); + // This is called by the controller manager before starting the controller. + void starting(); + // This is called by the controller manager before stopping the controller. + void stopping(); +protected: + // Counter for keeping track of controller steps. + int controller_counter_; + // Length of controller steps in ms. + int controller_step_length_; + // Position controller for passive arm. + boost::scoped_ptr nav_controller_; + // Linear velocities size + int linear_velocities_size_; + // Angular velocities size + int angular_velocities_size_; + // Action publisher + ros::Publisher cmd_pub_; + + // Initialize all of the ROS subscribers and publishers. + void initialize_ros(ros::NodeHandle& n); + // Initialize all of the sensors (this also includes FK computation objects). + void initialize_sensors(ros::NodeHandle& n); + // Initialize all of the position controllers. + void initialize_position_controllers(ros::NodeHandle& n); + + // Subscriber callbacks. + // Position command callback. + void nav_subscriber_callback(const gps_agent_pkg::NavigationCommand::ConstPtr& msg); + // Trial command callback. + void trial_subscriber_callback(const gps_agent_pkg::TrialCommand::ConstPtr& msg); + // Data request callback. + void data_request_subscriber_callback(const gps_agent_pkg::DataRequest::ConstPtr& msg); + + // Update functions. + // Update the sensors at each time step. + //void update_sensors(ros::Time current_time, bool is_controller_step); + // Update the controllers at each time step. + void update_controllers(ros::Time current_time, bool is_controller_step); + // Accessors. + // Get current time. + virtual ros::Time get_current_time() const; + // This should be empty + void get_joint_encoder_readings(Eigen::VectorXd &angles, gps::ActuatorType arm) const; +}; +} + +#endif /* GPS_AGENT_PKG_INCLUDE_GPS_AGENT_PKG_MOBILEROBOT_H_ */ diff --git a/src/gps_agent_pkg/include/gps_agent_pkg/mobilerobotsensor.h b/src/gps_agent_pkg/include/gps_agent_pkg/mobilerobotsensor.h new file mode 100644 index 000000000..8b928d328 --- /dev/null +++ b/src/gps_agent_pkg/include/gps_agent_pkg/mobilerobotsensor.h @@ -0,0 +1,143 @@ +/* + * mobilerobotsensor.h + * + * Created on: Mar 8, 2017 + * Author: thobotics + */ + +#ifndef GPS_AGENT_PKG_INCLUDE_MOBILEROBOTSENSOR_H_ +#define GPS_AGENT_PKG_INCLUDE_MOBILEROBOTSENSOR_H_ + +#include "gps/proto/gps.pb.h" + +// Superclass. +#include "gps_agent_pkg/sensor.h" +#include "gps_agent_pkg/sample.h" + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include "flann/flann.hpp" + +// This sensor writes to the following data types: +// MOBILE_POSITION +// MOBILE_ORIENTATION +// MOBILE_VELOCITIES_LINEAR +// MOBILE_VELOCITIES_ANGULAR +// POSITION_NEAREST_OBSTACLE + +using namespace std; + +namespace gps_control +{ +static const double PI= 3.1415926535897932384626433832795028841971693993751058209749445923078164062862089986280348; +static const double TWO_PI= 6.2831853071795864769252867665590057683943387987502116419498891846156328125724179972560696; +static const double minusPI= -3.1415926535897932384626433832795028841971693993751058209749445923078164062862089986280348; + +struct Point { + float a; + float b; + int member; + int p_idx; + Point(float x, float y) : a(x), b(y), member(-1), p_idx(0) {} + Point() : a(0), b(0), member(-1), p_idx(0) {} + inline bool operator==(Point p) { + if (p.a == a && p.b == b) + return true; + else + return false; + } +}; + +struct MinDistResult { + Point p; + double dist; +}; + +class MobileRobotSensor: public Sensor +{ +private: + // TODO: Does it need to change to ROS style + // Pose + Eigen::VectorXd position_; + Eigen::VectorXd orientation_; + // Velocities + Eigen::VectorXd linear_velocities_; + Eigen::VectorXd angular_velocities_; + // Position to nearest obstacle + Eigen::VectorXd nearest_obstacle_; + // Array of range sensor data + Eigen::VectorXd range_data_; + // Current global potential point + Eigen::VectorXd potential_score_; + // Subscribers + ros::Subscriber subscriber_; + ros::Subscriber range_subscriber_; + ros::Subscriber navfn_subscriber_; + // Publishers + ros::Publisher nearest_obs_pub_; + std::string topic_name_; + std::string range_topic_name_; + std::string potential_topic_name_; + + // Time from last update when the previous pose were recorded (necessary to compute velocities). + ros::Time previous_pose_time_; + + tf::TransformListener* tf_; + costmap_2d::Costmap2DROS* costmap_ros_; + boost::mutex odom_mutex_, range_mutex_; + geometry_msgs::Pose cur_pose_; + + // Building obstacle tree + boost::mutex cost_map_mutex_; + static char* cost_translation_table_; + nav_msgs::GridCells cost_map; + flann::Index > * obs_tree; + flann::Matrix * data; + + // Global Navigation Function + boost::mutex global_pot_mutex_; + std::vector global_potarr_; + unsigned int global_width_ = 0, global_height_ = 0; + double origin_x_ = 0, origin_y_ = 0, resolution_ = 0; + + // Auxiliary function + double mod(double x, double y); + double distance(double pose_x, double pose_y, double obx, double oby); + void updateObstacleTree(costmap_2d::Costmap2D *costmap); + geometry_msgs::Point transformOdomToMap(geometry_msgs::Pose local_pose); + double getGlobalPointPotential(geometry_msgs::Pose local_pose); + MinDistResult find_nearest_neighbor(Point queryPoint); + double min_distance_to_obstacle(geometry_msgs::Pose local_current_pose, double *heading, Point *obs_pose); + + // Subscriber topic + void update_data_vector(const nav_msgs::Odometry::ConstPtr& msg); + void update_range_data(const sensor_msgs::LaserScan::ConstPtr& msg); + void update_navfn(const nav_msgs::OccupancyGrid::ConstPtr& msg); + +public: + // Constructor. + MobileRobotSensor(ros::NodeHandle& n, RobotPlugin *plugin); + // Destructor. + virtual ~MobileRobotSensor(); + // Update the sensor (called every tick). + virtual void update(RobotPlugin *plugin, ros::Time current_time, bool is_controller_step); + // Configure the sensor (for sensor-specific trial settings). + virtual void configure_sensor(OptionsMap &options); + // Set data format and meta data on the provided sample. + virtual void set_sample_data_format(boost::scoped_ptr& sample); + // Set data on the provided sample. + virtual void set_sample_data(boost::scoped_ptr& sample, int t); +}; +} + + +#endif /* GPS_AGENT_PKG_INCLUDE_MOBILEROBOTSENSOR_H_ */ diff --git a/src/gps_agent_pkg/include/gps_agent_pkg/mpepccontrollaw.h b/src/gps_agent_pkg/include/gps_agent_pkg/mpepccontrollaw.h new file mode 100644 index 000000000..7a2120615 --- /dev/null +++ b/src/gps_agent_pkg/include/gps_agent_pkg/mpepccontrollaw.h @@ -0,0 +1,86 @@ +/* + * control_law.h + * + * Created on: Dec 2, 2016 + * Author: thobotics + */ + +#ifndef TURTLEBOT_MPEPC_MPEPC_LOCAL_PLANNER_INCLUDE_MPEPC_LOCAL_PLANNER_CONTROL_LAW_H_ +#define TURTLEBOT_MPEPC_MPEPC_LOCAL_PLANNER_INCLUDE_MPEPC_LOCAL_PLANNER_CONTROL_LAW_H_ + + +#include +#include +#include +#include +#include +#include +#include +#include + +#define LINEAR_THRESHOLD 0.05 +#define ANGULAR_THRESHOLD 0.05 +#define R_SPEED_LIMIT 0.5 + +using namespace std; +namespace mpepc_local_planner +{ + struct EgoPolar + { + double r; + double theta; + double delta; + }; + + struct ControlLawSettings + { + ControlLawSettings() { m_K1 = 2.0; m_K2 = 3.0; m_BETA = 0.4; m_LAMBDA = 2.0; m_R_THRESH = 1.5; m_R_THRESH_TRANS = 1.0; m_TRANS_INTERVAL = 2.3; m_V_MAX = 0.3; m_V_TRANS = 0.2; m_V_MIN = 0.05;} + public: + double m_K1; + double m_K2; // K1 and K2 are parameters that shape the planned manifold + double m_BETA; + double m_LAMBDA; // Beta and Lambda are parameters to shape the linear velocity rule + double m_R_THRESH; + double m_R_THRESH_TRANS; // The thresholds of r to begin slowing down or switch to v_trans + double m_TRANS_INTERVAL; // Time spent to complete the transition + double m_V_MAX; // Max allowable linear velocity + double m_V_TRANS; // Constant velocity used for transition + double m_V_MIN; // Minimum velocity (not included in formulation, but used for practical reasons on real platform) + double m_W_TURN; // Angular velocity for Goal tunning + }; + + class ControlLaw + { + public: + ControlLaw(); + explicit ControlLaw(ControlLawSettings* c); + geometry_msgs::Twist get_velocity_command(geometry_msgs::Pose current_position, geometry_msgs::Pose goal, double k1 = 2, double k2 = 3, double vMax = 0.3); + geometry_msgs::Twist get_velocity_command(EgoPolar goal_coords, double k1, double k2, double vMax); + geometry_msgs::Twist get_velocity_command(EgoPolar goal_coords, double vMax = 0.3); + double get_ego_distance(geometry_msgs::Pose current_position, geometry_msgs::Pose goal); + void update_k1_k2(double k1, double k2); + EgoPolar convert_to_egopolar(geometry_msgs::Pose current_pose, geometry_msgs::Pose current_goal_pose); + geometry_msgs::Pose convert_from_egopolar(geometry_msgs::Pose current_pose, EgoPolar current_goal_coords); + double wrap_pos_neg_pi(double angle); + + protected: + double get_kappa(EgoPolar current_ego_goal, double k1, double k2); + double get_linear_vel(double kappa, EgoPolar current_ego_goal, double vMax); + double get_angular_vel(double kappa, double linear_vel); + double calc_sigmoid(double time_tau); + + private: + static const double _PI= 3.1415926535897932384626433832795028841971693993751058209749445923078164062862089986280348; + static const double _TWO_PI= 6.2831853071795864769252867665590057683943387987502116419498891846156328125724179972560696; + + double mod(double x, double y); + + + ControlLawSettings* settings_; + }; +} + + + + +#endif /* TURTLEBOT_MPEPC_MPEPC_LOCAL_PLANNER_INCLUDE_MPEPC_LOCAL_PLANNER_CONTROL_LAW_H_ */ diff --git a/src/gps_agent_pkg/include/gps_agent_pkg/mpepcmobilerobot.h b/src/gps_agent_pkg/include/gps_agent_pkg/mpepcmobilerobot.h new file mode 100644 index 000000000..5b53acc18 --- /dev/null +++ b/src/gps_agent_pkg/include/gps_agent_pkg/mpepcmobilerobot.h @@ -0,0 +1,38 @@ +/* + * mpepcmobilerobot.h + * + * Created on: Feb 20, 2018 + * Author: thobotics + */ + +#ifndef GPS_AGENT_PKG_INCLUDE_GPS_AGENT_PKG_MPEPCMOBILEROBOT_H_ +#define GPS_AGENT_PKG_INCLUDE_GPS_AGENT_PKG_MPEPCMOBILEROBOT_H_ + + +#include +#include +#include +#include "gps_agent_pkg/mobilerobot.h" +#include "gps/proto/gps.pb.h" + +namespace gps_control +{ + +class MpepcMobileRobot: public MobileRobot { +public: + // Constructor (this should do nothing). + MpepcMobileRobot(); + // Destructor. + virtual ~MpepcMobileRobot(); + + // Init all things needed + void init(ros::NodeHandle& n); + // This is the main update function called by the realtime thread when the controller is running. + void update(); +private: + // Action publisher + ros::Publisher ego_goal_pub_; +}; +} + +#endif /* GPS_AGENT_PKG_INCLUDE_GPS_AGENT_PKG_MPEPCMOBILEROBOT_H_ */ diff --git a/src/gps_agent_pkg/include/gps_agent_pkg/navcontroller.h b/src/gps_agent_pkg/include/gps_agent_pkg/navcontroller.h new file mode 100644 index 000000000..3be5377df --- /dev/null +++ b/src/gps_agent_pkg/include/gps_agent_pkg/navcontroller.h @@ -0,0 +1,52 @@ +/* + * navcontroller.h + * + * Created on: Mar 8, 2017 + * Author: thobotics + */ + +#ifndef GPS_AGENT_PKG_INCLUDE_GPS_AGENT_PKG_NAVCONTROLLER_H_ +#define GPS_AGENT_PKG_INCLUDE_GPS_AGENT_PKG_NAVCONTROLLER_H_ + +// Headers. +#include + +// Superclass. +#include "gps_agent_pkg/controller.h" +#include "gps/proto/gps.pb.h" +#include + +namespace gps_control +{ + +class NavController : public Controller +{ +private: + // Pose + Eigen::VectorXd position_; + Eigen::VectorXd orientation_; + // Agent move_base publisher + ros::Publisher nav_pub_; + // TODO: Change this by checking error + bool finished_; +public: + // Constructor. + NavController(ros::NodeHandle& n); + // Destructor. + virtual ~NavController(); + // Update the controller (take an action). + virtual void update(RobotPlugin *plugin, ros::Time current_time, boost::scoped_ptr& sample, Eigen::VectorXd &torques); + // Configure the controller. + virtual void configure_controller(OptionsMap &options); + // Check if controller is finished with its current task. + virtual bool is_finished() const; + // Reset the controller -- this is typically called when the controller is turned on. + virtual void reset(ros::Time update_time); + // Should this report when position achieved? + bool report_waiting; +}; +} + + + +#endif /* GPS_AGENT_PKG_INCLUDE_GPS_AGENT_PKG_NAVCONTROLLER_H_ */ diff --git a/src/gps_agent_pkg/include/gps_agent_pkg/sensor.h b/src/gps_agent_pkg/include/gps_agent_pkg/sensor.h index 1d9d65c77..1f2d622d6 100755 --- a/src/gps_agent_pkg/include/gps_agent_pkg/sensor.h +++ b/src/gps_agent_pkg/include/gps_agent_pkg/sensor.h @@ -25,6 +25,7 @@ enum SensorType EncoderSensorType = 0, ROSTopicSensorType = 1, CameraSensorType, + MobileSensorType, TotalSensorTypes }; diff --git a/src/gps_agent_pkg/launch/include/world_stage.launch b/src/gps_agent_pkg/launch/include/world_stage.launch new file mode 100644 index 000000000..ab2f072ba --- /dev/null +++ b/src/gps_agent_pkg/launch/include/world_stage.launch @@ -0,0 +1,94 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/src/gps_agent_pkg/launch/mobilerobot_gps.launch b/src/gps_agent_pkg/launch/mobilerobot_gps.launch new file mode 100644 index 000000000..f46867404 --- /dev/null +++ b/src/gps_agent_pkg/launch/mobilerobot_gps.launch @@ -0,0 +1,21 @@ + + + + + + + + + + + + + + + + + + + + + diff --git a/src/gps_agent_pkg/launch/mobilerobot_mpepc_gps.launch b/src/gps_agent_pkg/launch/mobilerobot_mpepc_gps.launch new file mode 100644 index 000000000..06cf32d8f --- /dev/null +++ b/src/gps_agent_pkg/launch/mobilerobot_mpepc_gps.launch @@ -0,0 +1,31 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/src/gps_agent_pkg/launch/pr2_gazebo_no_controller.launch b/src/gps_agent_pkg/launch/pr2_gazebo_no_controller.launch index a01c85d24..65298461e 100644 --- a/src/gps_agent_pkg/launch/pr2_gazebo_no_controller.launch +++ b/src/gps_agent_pkg/launch/pr2_gazebo_no_controller.launch @@ -1,6 +1,6 @@ - + diff --git a/src/gps_agent_pkg/manifest.xml b/src/gps_agent_pkg/manifest.xml index b0d983dad..1aa6b501c 100644 --- a/src/gps_agent_pkg/manifest.xml +++ b/src/gps_agent_pkg/manifest.xml @@ -7,6 +7,7 @@ + @@ -19,7 +20,9 @@ - + + + diff --git a/src/gps_agent_pkg/maps/dynamic_obstacle_ext.png b/src/gps_agent_pkg/maps/dynamic_obstacle_ext.png new file mode 100644 index 000000000..9eb98b440 Binary files /dev/null and b/src/gps_agent_pkg/maps/dynamic_obstacle_ext.png differ diff --git a/src/gps_agent_pkg/maps/hallway.png b/src/gps_agent_pkg/maps/hallway.png new file mode 100644 index 000000000..bd0e98dff Binary files /dev/null and b/src/gps_agent_pkg/maps/hallway.png differ diff --git a/src/gps_agent_pkg/maps/hallway.yaml b/src/gps_agent_pkg/maps/hallway.yaml new file mode 100644 index 000000000..feee4ec59 --- /dev/null +++ b/src/gps_agent_pkg/maps/hallway.yaml @@ -0,0 +1,6 @@ +image: hallway.png +resolution: 0.05 +origin: [0.0, 0.0, 0.0] +negate: 0 +occupied_thresh: 0.65 +free_thresh: 0.196 diff --git a/src/gps_agent_pkg/maps/hallway_bend.png b/src/gps_agent_pkg/maps/hallway_bend.png new file mode 100644 index 000000000..1fd8aa8ec Binary files /dev/null and b/src/gps_agent_pkg/maps/hallway_bend.png differ diff --git a/src/gps_agent_pkg/maps/hallway_bend.yaml b/src/gps_agent_pkg/maps/hallway_bend.yaml new file mode 100644 index 000000000..5c09a1eca --- /dev/null +++ b/src/gps_agent_pkg/maps/hallway_bend.yaml @@ -0,0 +1,6 @@ +image: hallway_bend.png +resolution: 0.05 +origin: [0.0, 0.0, 0.0] +negate: 0 +occupied_thresh: 0.65 +free_thresh: 0.196 diff --git a/src/gps_agent_pkg/maps/one_obstacle.yaml b/src/gps_agent_pkg/maps/one_obstacle.yaml new file mode 100644 index 000000000..e3a4bd581 --- /dev/null +++ b/src/gps_agent_pkg/maps/one_obstacle.yaml @@ -0,0 +1,6 @@ +image: dynamic_obstacle_ext.png +resolution: 0.05 +origin: [0.0, 0.0, 0.0] +negate: 0 +occupied_thresh: 0.65 +free_thresh: 0.196 diff --git a/src/gps_agent_pkg/msg/EgoGoal.msg b/src/gps_agent_pkg/msg/EgoGoal.msg new file mode 100644 index 000000000..acc42a9d2 --- /dev/null +++ b/src/gps_agent_pkg/msg/EgoGoal.msg @@ -0,0 +1,6 @@ +float32 r +float32 theta +float32 delta +float32 vMax +float32 k1 +float32 k2 \ No newline at end of file diff --git a/src/gps_agent_pkg/msg/MobileCommand.msg b/src/gps_agent_pkg/msg/MobileCommand.msg new file mode 100644 index 000000000..734f66351 --- /dev/null +++ b/src/gps_agent_pkg/msg/MobileCommand.msg @@ -0,0 +1,12 @@ +# This message is published to the C++ controller to start +# a mobile robot +int32 id # ID must be echoed back in SampleResult +ControllerParams controller + +# Trial information +int32 T # Trajectory length +float64 frequency # Controller frequency +int8[] state_datatypes # Which data types to include in state +int8[] obs_datatypes # Which data types to include in observation +float64[] position_tgt # A 3x1 array containing target position +float64[] quaternion_tgt # A 4x1 array containing target orientation diff --git a/src/gps_agent_pkg/msg/NavigationCommand.msg b/src/gps_agent_pkg/msg/NavigationCommand.msg new file mode 100644 index 000000000..0737edfa2 --- /dev/null +++ b/src/gps_agent_pkg/msg/NavigationCommand.msg @@ -0,0 +1,3 @@ +int32 id # ID must be echoed back in SampleResult +float64[] position +float64[] quaternion diff --git a/src/gps_agent_pkg/param/costmap_common_params.yaml b/src/gps_agent_pkg/param/costmap_common_params.yaml new file mode 100644 index 000000000..e9276649d --- /dev/null +++ b/src/gps_agent_pkg/param/costmap_common_params.yaml @@ -0,0 +1,53 @@ +max_obstacle_height: 0.60 # assume something like an arm is mounted on top of the robot + +# Obstacle Cost Shaping (http://wiki.ros.org/costmap_2d/hydro/inflation) +robot_radius: 0.20 # distance a circular robot should be clear of the obstacle (kobuki: 0.18) +# footprint: [[x0, y0], [x1, y1], ... [xn, yn]] # if the robot is not circular + +map_type: voxel + +obstacle_layer: + enabled: true + max_obstacle_height: 0.6 + origin_z: 0.0 + z_resolution: 0.2 + z_voxels: 2 + unknown_threshold: 15 + mark_threshold: 0 + combination_method: 1 + track_unknown_space: true #true needed for disabling global path planning through unknown space + obstacle_range: 2.5 + raytrace_range: 3.0 + origin_z: 0.0 + z_resolution: 0.2 + z_voxels: 2 + publish_voxel_map: false + observation_sources: scan bump + global_frame: odom + robot_base_frame: /base_footprint + scan: + data_type: LaserScan + topic: scan + marking: true + clearing: true + min_obstacle_height: 0.25 + max_obstacle_height: 0.35 + bump: + data_type: PointCloud2 + topic: mobile_base/sensors/bumper_pointcloud + marking: true + clearing: false + min_obstacle_height: 0.0 + max_obstacle_height: 0.15 + # for debugging only, let's you see the entire voxel grid + +#cost_scaling_factor and inflation_radius were now moved to the inflation_layer ns +inflation_layer: + enabled: true + cost_scaling_factor: 5.0 # exponential rate at which the obstacle cost drops off (default: 10) + inflation_radius: 0.5 # max. distance from an obstacle at which costs are incurred for planning paths. + +static_layer: + enabled: true + + diff --git a/src/gps_agent_pkg/param/global_costmap_params.yaml b/src/gps_agent_pkg/param/global_costmap_params.yaml new file mode 100644 index 000000000..56b23c780 --- /dev/null +++ b/src/gps_agent_pkg/param/global_costmap_params.yaml @@ -0,0 +1,12 @@ +global_costmap: + global_frame: /map + robot_base_frame: /base_footprint + update_frequency: 1.0 + publish_frequency: 0.5 + static_map: true + transform_tolerance: 0.5 + plugins: + - {name: static_layer, type: "costmap_2d::StaticLayer"} + - {name: obstacle_layer, type: "costmap_2d::VoxelLayer"} + - {name: inflation_layer, type: "costmap_2d::InflationLayer"} + diff --git a/src/gps_agent_pkg/param/global_planner_params.yaml b/src/gps_agent_pkg/param/global_planner_params.yaml new file mode 100644 index 000000000..6b8c2ee21 --- /dev/null +++ b/src/gps_agent_pkg/param/global_planner_params.yaml @@ -0,0 +1,20 @@ + +GlobalPlanner: # Also see: http://wiki.ros.org/global_planner + old_navfn_behavior: false # Exactly mirror behavior of navfn, use defaults for other boolean parameters, default false + use_quadratic: true # Use the quadratic approximation of the potential. Otherwise, use a simpler calculation, default true + use_dijkstra: true # Use dijkstra's algorithm. Otherwise, A*, default true + use_grid_path: false # Create a path that follows the grid boundaries. Otherwise, use a gradient descent method, default false + + allow_unknown: true # Allow planner to plan through unknown space, default true + #Needs to have track_unknown_space: true in the obstacle / voxel layer (in costmap_commons_param) to work + planner_window_x: 0.0 # default 0.0 + planner_window_y: 0.0 # default 0.0 + default_tolerance: 0.0 # If goal in obstacle, plan to the closest point in radius default_tolerance, default 0.0 + + publish_scale: 100 # Scale by which the published potential gets multiplied, default 100 + planner_costmap_publish_frequency: 0.0 # default 0.0 + + lethal_cost: 253 # default 253 + neutral_cost: 50 # default 50 + cost_factor: 3.0 # Factor to multiply each cost from costmap by, default 3.0 + publish_potential: true # Publish Potential Costmap (this is not like the navfn pointcloud2 potential), default true diff --git a/src/gps_agent_pkg/param/local_costmap_params.yaml b/src/gps_agent_pkg/param/local_costmap_params.yaml new file mode 100644 index 000000000..60469e320 --- /dev/null +++ b/src/gps_agent_pkg/param/local_costmap_params.yaml @@ -0,0 +1,14 @@ +local_costmap: + global_frame: odom + robot_base_frame: /base_footprint + update_frequency: 10.0 + publish_frequency: 2.0 + static_map: false + rolling_window: true + width: 5.0 + height: 5.0 + resolution: 0.05 + transform_tolerance: 0.5 + plugins: + - {name: obstacle_layer, type: "costmap_2d::VoxelLayer"} + - {name: inflation_layer, type: "costmap_2d::InflationLayer"} diff --git a/src/gps_agent_pkg/param/turtlebot.rviz b/src/gps_agent_pkg/param/turtlebot.rviz new file mode 100644 index 000000000..194aa60ea --- /dev/null +++ b/src/gps_agent_pkg/param/turtlebot.rviz @@ -0,0 +1,461 @@ +Panels: + - Class: rviz/Displays + Help Height: 81 + Name: Displays + Property Tree Widget: + Expanded: + - /TF1/Frames1 + - /TF1/Tree1 + Splitter Ratio: 0.5 + Tree Height: 222 + - Class: rviz/Selection + Name: Selection + - Class: rviz/Tool Properties + Expanded: + - /2D Pose Estimate1 + - /2D Nav Goal1 + Name: Tool Properties + Splitter Ratio: 0.588679 + - Class: rviz/Views + Expanded: + - /Current View1 + Name: Views + Splitter Ratio: 0.5 + - Class: rviz/Time + Experimental: false + Name: Time + SyncMode: 0 + SyncSource: LaserScan (kinect) +Visualization Manager: + Class: "" + Displays: + - Alpha: 0.5 + Cell Size: 1 + Class: rviz/Grid + Color: 160; 160; 164 + Enabled: true + Line Style: + Line Width: 0.03 + Value: Lines + Name: Grid + Normal Cell Count: 0 + Offset: + X: 0 + Y: 0 + Z: 0 + Plane: XY + Plane Cell Count: 10 + Reference Frame: + Value: true + - Alpha: 1 + Class: rviz/RobotModel + Collision Enabled: false + Enabled: true + Links: + All Links Enabled: true + Expand Joint Details: false + Expand Link Details: false + Expand Tree: false + Link Tree Style: Links in Alphabetic Order + base_footprint: + Alpha: 1 + Show Axes: false + Show Trail: false + base_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + camera_depth_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + camera_depth_optical_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + camera_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + camera_rgb_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + camera_rgb_optical_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + caster_back_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + caster_front_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + cliff_sensor_front_link: + Alpha: 1 + Show Axes: false + Show Trail: false + cliff_sensor_left_link: + Alpha: 1 + Show Axes: false + Show Trail: false + cliff_sensor_right_link: + Alpha: 1 + Show Axes: false + Show Trail: false + gyro_link: + Alpha: 1 + Show Axes: false + Show Trail: false + mount_asus_xtion_pro_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + plate_bottom_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + plate_middle_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + plate_top_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + pole_bottom_0_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + pole_bottom_1_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + pole_bottom_2_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + pole_bottom_3_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + pole_bottom_4_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + pole_bottom_5_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + pole_kinect_0_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + pole_kinect_1_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + pole_middle_0_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + pole_middle_1_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + pole_middle_2_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + pole_middle_3_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + pole_top_0_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + pole_top_1_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + pole_top_2_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + pole_top_3_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + wheel_left_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + wheel_right_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + Name: RobotModel + Robot Description: robot_description + TF Prefix: "" + Update Interval: 0 + Value: true + Visual Enabled: true + - Class: rviz/TF + Enabled: false + Frame Timeout: 15 + Frames: + All Enabled: false + Marker Scale: 1 + Name: TF + Show Arrows: true + Show Axes: true + Show Names: false + Tree: + {} + Update Interval: 0 + Value: false + - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 10 + Min Value: -10 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz/LaserScan + Color: 255; 255; 255 + Color Transformer: Intensity + Decay Time: 0 + Enabled: true + Invert Rainbow: false + Max Color: 255; 255; 255 + Max Intensity: 1 + Min Color: 0; 0; 0 + Min Intensity: 1 + Name: LaserScan (kinect) + Position Transformer: XYZ + Queue Size: 10 + Selectable: true + Size (Pixels): 3 + Size (m): 0.1 + Style: Squares + Topic: /scan + Use Fixed Frame: true + Use rainbow: true + Value: true + - Alpha: 0.7 + Class: rviz/Map + Color Scheme: map + Draw Behind: false + Enabled: true + Name: Map + Topic: /map + Value: true + - Alpha: 0.7 + Class: rviz/Map + Color Scheme: map + Draw Behind: false + Enabled: true + Name: CostMap + Topic: /mobilerobot_node/local_costmap/costmap + Value: true + - Angle Tolerance: 0.1 + Class: rviz/Odometry + Color: 48; 48; 48 + Enabled: true + Keep: 100 + Length: 0.4 + Name: Odometry + Position Tolerance: 0.1 + Topic: /odom + Value: true + - Class: rviz/Camera + Enabled: true + Image Rendering: background and overlay + Image Topic: /image + Name: Camera + Overlay Alpha: 0.5 + Queue Size: 2 + Transport Hint: raw + Value: true + Visibility: + CostMap: true + Global CostMap: true + Global Path: true + Goal Pose: true + Grid: true + InterGoal Pose: true + LaserScan (kinect): true + MPC Plan: true + Map: true + Nearest obstacle: true + Odometry: true + Offline Plan: true + Potential Map: true + RobotModel: true + TF: true + Value: true + Zoom Factor: 1 + - Arrow Length: 0.3 + Class: rviz/PoseArray + Color: 255; 170; 0 + Enabled: true + Name: Offline Plan + Topic: /ofline_plan + Value: true + - Arrow Length: 0.3 + Class: rviz/PoseArray + Color: 255; 255; 0 + Enabled: true + Name: MPC Plan + Topic: /mpc_plan + Value: true + - Alpha: 0.7 + Class: rviz/Map + Color Scheme: costmap + Draw Behind: false + Enabled: true + Name: Global CostMap + Topic: /globalplanner_node/global_costmap/costmap + Value: true + - Alpha: 1 + Axes Length: 1 + Axes Radius: 0.1 + Class: rviz/Pose + Color: 255; 25; 0 + Enabled: true + Head Length: 0.3 + Head Radius: 0.1 + Name: Goal Pose + Shaft Length: 1 + Shaft Radius: 0.05 + Shape: Arrow + Topic: /global_goal + Value: true + - Alpha: 1 + Buffer Length: 1 + Class: rviz/Path + Color: 25; 255; 0 + Enabled: true + Line Style: Lines + Line Width: 0.03 + Name: Global Path + Offset: + X: 0 + Y: 0 + Z: 0 + Topic: /globalplanner_node/GlobalPlanner/plan + Value: true + - Alpha: 0.7 + Class: rviz/Map + Color Scheme: jet (101 levels) + Draw Behind: true + Enabled: true + Name: Potential Map + Topic: /globalplanner_node/GlobalPlanner/potential + Value: true + - Class: rviz/Marker + Enabled: true + Marker Topic: /nearest_obstacle + Name: Nearest obstacle + Namespaces: + nearest_points: true + Queue Size: 100 + Value: true + - Alpha: 1 + Axes Length: 0.5 + Axes Radius: 0.1 + Class: rviz/Pose + Color: 255; 25; 0 + Enabled: true + Head Length: 0.3 + Head Radius: 0.1 + Name: InterGoal Pose + Shaft Length: 1 + Shaft Radius: 0.05 + Shape: Axes + Topic: /inter_goal + Value: true + Enabled: true + Global Options: + Background Color: 48; 48; 48 + Fixed Frame: map + Frame Rate: 30 + Name: root + Tools: + - Class: rviz/MoveCamera + - Class: rviz/Interact + Hide Inactive Objects: true + - Class: rviz/Select + - Class: rviz/SetInitialPose + Topic: /initialpose + - Class: rviz/SetGoal + Topic: /move_base_simple/goal + - Class: rviz/Measure + Value: true + Views: + Current: + Angle: 0 + Class: rviz/TopDownOrtho + Enable Stereo Rendering: + Stereo Eye Separation: 0.06 + Stereo Focal Distance: 1 + Swap Stereo Eyes: false + Value: false + Name: Current View + Near Clip Distance: 0.01 + Scale: 33.7909 + Target Frame: base_link + Value: TopDownOrtho (rviz) + X: 1.53197 + Y: 0.599903 + Saved: ~ +Window Geometry: + Camera: + collapsed: false + Displays: + collapsed: false + Height: 609 + Hide Left Dock: false + Hide Right Dock: true + QMainWindow State: 000000ff00000000fd00000004000000000000016a0000020afc0200000006fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006700fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d00000170000000e300fffffffb0000000c00430061006d00650072006101000001b3000000940000001600ffffff000000010000010f0000020afc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003d0000020a000000b300fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004a00000003efc0100000002fb0000000800540069006d00650000000000000004a00000028600fffffffb0000000800540069006d006501000000000000045000000000000000000000025e0000020a00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + Selection: + collapsed: false + Time: + collapsed: false + Tool Properties: + collapsed: false + Views: + collapsed: true + Width: 974 + X: 70 + Y: 98 diff --git a/src/gps_agent_pkg/src/controllaw_node.cpp b/src/gps_agent_pkg/src/controllaw_node.cpp new file mode 100644 index 000000000..93c09a473 --- /dev/null +++ b/src/gps_agent_pkg/src/controllaw_node.cpp @@ -0,0 +1,159 @@ +/* + * controllaw_node.cpp + * + * Created on: Feb 19, 2018 + * Author: thobotics + */ +#include +#include +#include +#include +#include "gps_agent_pkg/EgoGoal.h" +#include "gps_agent_pkg/mpepccontrollaw.h" + +using namespace mpepc_local_planner; + +tf::TransformListener* tf_; +ros::Publisher cmd_vel_pub_, inter_goal_pub_; +EgoPolar inter_goal_coords_; +double inter_goal_vMax_, inter_goal_k1_, inter_goal_k2_; +double GOAL_DIST_UPDATE_THRESH; +double GOAL_ANGLE_UPDATE_THRESH; +geometry_msgs::Pose local_goal_pose_; +bool is_goal_received = false; +boost::mutex inter_goal_mutex_; +ControlLawSettings settings_; +ControlLaw* cl; + +void egoGoalCallback(const gps_agent_pkg::EgoGoal::ConstPtr& ego_goal) +{ + boost::mutex::scoped_lock lock(inter_goal_mutex_); + ROS_INFO("EgoGoal: [%f, %f, %f]", ego_goal->r, ego_goal->delta, ego_goal->theta); + inter_goal_coords_.r = ego_goal->r; + inter_goal_coords_.delta = ego_goal->delta; + inter_goal_coords_.theta = ego_goal->theta; + inter_goal_vMax_ = ego_goal->vMax; + inter_goal_k1_ = ego_goal->k1; + inter_goal_k2_ = ego_goal->k2; +} + +void globalGoalCallback(const geometry_msgs::PoseStamped::ConstPtr& global_goal_pose) +{ + geometry_msgs::PoseStamped local_pose_stamp; + try{ + tf_->waitForTransform("/odom", "/map", global_goal_pose->header.stamp, ros::Duration(10.0)); + tf_->transformPose("/odom", *global_goal_pose, local_pose_stamp); + }catch (tf::TransformException & ex){ + ROS_ERROR("Transform exception 222 : %s", ex.what()); + } + + local_goal_pose_ = local_pose_stamp.pose; + + ROS_INFO("Local goal update: [%f, %f] ", + local_goal_pose_.position.x, local_goal_pose_.position.y); + is_goal_received = true; +} + +void getCurrentRobotPose(geometry_msgs::Pose &pose){ + tf::StampedTransform tfTransform; + try { + tf_->lookupTransform("odom", "base_footprint", + ros::Time(0), tfTransform); + } catch (tf::TransformException& e) { + ROS_WARN_STREAM_THROTTLE( + 5.0, + "TF lookup from base_footprint to odom failed. Reason: " << e.what()); + return; + } + + tf::pointTFToMsg(tfTransform.getOrigin(), pose.position); + tf::quaternionTFToMsg(tfTransform.getRotation(), pose.orientation); +} + +void computeVelocity(geometry_msgs::Pose ¤t_pose, geometry_msgs::Twist &cmd_vel){ + EgoPolar global_goal_coords; + global_goal_coords = cl->convert_to_egopolar(current_pose, local_goal_pose_); + + ROS_DEBUG("Distance to goal: %f", global_goal_coords.r); + if (global_goal_coords.r <= GOAL_DIST_UPDATE_THRESH){ + double angle_error = tf::getYaw(current_pose.orientation) - tf::getYaw(local_goal_pose_.orientation); + angle_error = cl->wrap_pos_neg_pi(angle_error); + ROS_DEBUG("Angle error: %f", angle_error); + + if (fabs(angle_error) > GOAL_ANGLE_UPDATE_THRESH) + { + cmd_vel.linear.x = 0; + if (angle_error > 0) + cmd_vel.angular.z = -4 * settings_.m_W_TURN; + else + cmd_vel.angular.z = 4 * settings_.m_W_TURN; + } + else + { + ROS_INFO("[MPEPC] Completed normal trajectory following"); +// goal_reached_ = true; + cmd_vel.linear.x = 0; + cmd_vel.angular.z = 0; + } + } else { + { + boost::mutex::scoped_lock lock(inter_goal_mutex_); + if(inter_goal_coords_.r > 0){ // Used to != -1 + geometry_msgs::Pose inter_goal_pose = cl->convert_from_egopolar(current_pose, inter_goal_coords_); + cmd_vel = cl->get_velocity_command(current_pose, inter_goal_pose, inter_goal_k1_, inter_goal_k2_, inter_goal_vMax_); + + geometry_msgs::PoseStamped int_goal_stamp; + int_goal_stamp.header.frame_id = "odom"; + int_goal_stamp.header.stamp = ros::Time(0); + int_goal_stamp.pose = inter_goal_pose; + inter_goal_pub_.publish(int_goal_stamp); + }else{ + cmd_vel.linear.x = 0; + cmd_vel.angular.z = 0; + } + } + } +} + +int main(int argc, char** argv){ + ros::init(argc, argv, "controllaw_node"); + ros::NodeHandle nh; + ros::NodeHandle private_nh("~"); + ros::Rate rate(20); + tf_ = new tf::TransformListener(ros::Duration(10)); + + cmd_vel_pub_ = nh.advertise("/cmd_vel_mux/input/navi", 1000); + inter_goal_pub_ = nh.advertise("inter_goal", 1000); + ros::Subscriber ego_goal_sub = nh.subscribe("ego_goal", 1, egoGoalCallback); + ros::Subscriber global_goal_sub = nh.subscribe("global_goal", 1, globalGoalCallback); + + private_nh.param("K_1", settings_.m_K1, 2.0); + private_nh.param("K_2", settings_.m_K2, 3.0); + private_nh.param("BETA", settings_.m_BETA, 0.4); + private_nh.param("LAMBDA", settings_.m_LAMBDA, 2.0); + private_nh.param("R_THRESH", settings_.m_R_THRESH, 0.05); + private_nh.param("V_MAX", settings_.m_V_MAX, 1.0); + private_nh.param("V_MIN", settings_.m_V_MIN, 0.0); + private_nh.param("W_TURN", settings_.m_W_TURN, 0.0); + private_nh.param("goal_dist_tol", GOAL_DIST_UPDATE_THRESH, 0.15); + private_nh.param("goal_angle_tol", GOAL_ANGLE_UPDATE_THRESH, 0.1); + + cl = new ControlLaw(&settings_); + geometry_msgs::Twist cmd_vel; + geometry_msgs::Pose current_pose; + + // Control Loop and code + while (ros::ok()) + { + getCurrentRobotPose(current_pose); + if(is_goal_received){ // Make sure received first ever goal + computeVelocity(current_pose, cmd_vel); + cmd_vel_pub_.publish(cmd_vel); + } + + ros::spinOnce(); + rate.sleep(); + } + + return 0; +} diff --git a/src/gps_agent_pkg/src/globalplanner_node.cpp b/src/gps_agent_pkg/src/globalplanner_node.cpp new file mode 100644 index 000000000..a3aaf6dbc --- /dev/null +++ b/src/gps_agent_pkg/src/globalplanner_node.cpp @@ -0,0 +1,79 @@ +/* + * globalplanner_node.cpp + * + * Created on: Feb 17, 2018 + * Author: thobotics + */ +#include +#include +#include +#include + +costmap_2d::Costmap2DROS* costmap; +global_planner::GlobalPlanner* planner; +std::vector global_plan; +geometry_msgs::PoseStamped global_goal; + +bool makePlan(const geometry_msgs::PoseStamped& goal, std::vector& plan){ + boost::unique_lock lock(*(costmap->getCostmap()->getMutex())); + + //make sure to set the plan to be empty initially + plan.clear(); + + //since this gets called on handle activate + if(costmap == NULL) { + ROS_ERROR("Planner costmap ROS is NULL, unable to create global plan"); + return false; + } + + //get the starting pose of the robot + tf::Stamped global_pose; + if(!costmap->getRobotPose(global_pose)) { + ROS_WARN("Unable to get starting pose of robot, unable to create global plan"); + return false; + } + + geometry_msgs::PoseStamped start; + tf::poseStampedTFToMsg(global_pose, start); + + //if the planner fails or returns a zero length plan, planning failed + if(!planner->makePlan(goal, start, plan) || plan.empty()){ + ROS_DEBUG_NAMED("move_base","Failed to find a plan to point (%.2f, %.2f)", goal.pose.position.x, goal.pose.position.y); + return false; + } + + // Reverse the plan + geometry_msgs::PoseStamped goal_copy = goal; + goal_copy.header.stamp = goal.header.stamp; + plan[0] = goal_copy; + std::reverse(plan.begin(), plan.end()); + + return true; +} + +void goalCallback(const geometry_msgs::PoseStamped::ConstPtr& goal) +{ + ROS_INFO("Goal: [%f, %f]", goal->pose.position.x, goal->pose.position.y); + global_goal.header = goal->header; + global_goal.pose = goal->pose; + + if(global_goal.header.frame_id != "" && makePlan(global_goal, global_plan)){ + ROS_INFO("Planned"); + } +} + +int main(int argc, char** argv){ + ros::init(argc, argv, "globalplanner_node"); + ros::NodeHandle n; + tf::TransformListener tf(ros::Duration(10)); + + costmap = new costmap_2d::Costmap2DROS("global_costmap", tf); + planner = new global_planner::GlobalPlanner(); + planner->initialize("GlobalPlanner", costmap); + + ros::Subscriber sub = n.subscribe("global_goal", 1, goalCallback); + + ros::spin(); + + return 0; +} diff --git a/src/gps_agent_pkg/src/mobilerobot.cpp b/src/gps_agent_pkg/src/mobilerobot.cpp new file mode 100644 index 000000000..1e43c7ea5 --- /dev/null +++ b/src/gps_agent_pkg/src/mobilerobot.cpp @@ -0,0 +1,241 @@ +/* + * mobilerobot.cpp + * + * Created on: Mar 5, 2017 + * Author: thobotics + */ +#include "gps_agent_pkg/mobilerobot.h" +#include "gps_agent_pkg/sensor.h" +#include "gps_agent_pkg/controller.h" +#include "gps_agent_pkg/positioncontroller.h" +#include "gps_agent_pkg/lingausscontroller.h" +#include "gps_agent_pkg/trialcontroller.h" +#include "gps_agent_pkg/LinGaussParams.h" +#include "gps_agent_pkg/ControllerParams.h" +#include "gps_agent_pkg/util.h" +#include "gps/proto/gps.pb.h" +#include + +#ifdef USE_CAFFE +#include "gps_agent_pkg/caffenncontroller.h" +#include "gps_agent_pkg/CaffeParams.h" +#endif + +using namespace gps_control; + +// Plugin constructor. +MobileRobot::MobileRobot() +{ + // Nothing to do here, since all variables are initialized in initialize(...) + // Some basic variable initialization. + controller_counter_ = 0; + controller_step_length_ = 4; +} + +// Destructor. +MobileRobot::~MobileRobot() +{ + // Nothing to do here, since all instance variables are destructed automatically. +} + +void MobileRobot::init(ros::NodeHandle& n) +{ + // TODO: Set topic name by parameters + cmd_pub_ = n.advertise("/cmd_vel_mux/input/navi", 1); + // TODO: Set this by parameters + linear_velocities_size_ = 2; // Vx, Vy + angular_velocities_size_ = 1; // Wz (yaw) + + // Action is linear and angular velocity + active_arm_torques_.resize(linear_velocities_size_+angular_velocities_size_); + + // Initialize ROS subscribers/publishers, sensors, and navigation controllers. + initialize(n); +} + +// This is called by the controller manager before starting the controller. +void MobileRobot::starting() +{ + // Get current time. + last_update_time_ = ros::Time::now(); + controller_counter_ = 0; + + // Reset all the sensors. This is important for sensors that try to keep + // track of the previous state somehow. + //for (int sensor = 0; sensor < TotalSensorTypes; sensor++) + /*for (int sensor = 0; sensor < 1; sensor++) + { + sensors_[sensor]->reset(this,last_update_time_); + }*/ + + // Reset position controllers. + nav_controller_->reset(last_update_time_); + + // Reset trial controller, if any. + if (trial_controller_ != NULL) trial_controller_->reset(last_update_time_); +} + +// This is called by the controller manager before stopping the controller. +void MobileRobot::stopping() +{ + // Nothing to do here. +} + +void MobileRobot::update() +{ + // Get current time. + last_update_time_ = ros::Time::now(); + + // Check if this is a controller step based on the current controller frequency. + controller_counter_++; + if (controller_counter_ >= controller_step_length_) controller_counter_ = 0; + bool is_controller_step = (controller_counter_ == 0); + + // Update the sensors and fill in the current step sample. + update_sensors(last_update_time_,is_controller_step); + + // Update the controllers. + update_controllers(last_update_time_,is_controller_step); + + // Setup action and send to robot + geometry_msgs::Twist cmd_vel; + cmd_vel.linear.x = active_arm_torques_[0]; + cmd_vel.linear.y = active_arm_torques_[1]; + cmd_vel.angular.z = active_arm_torques_[2]; + cmd_pub_.publish(cmd_vel); +} + +// Initialize ROS communication infrastructure. +void MobileRobot::initialize_ros(ros::NodeHandle& n) +{ + ROS_INFO_STREAM("Initializing mobile robot ROS subs/pubs"); + // Create subscribers. + position_subscriber_ = n.subscribe("/gps_controller_navigation_command", 1, &MobileRobot::nav_subscriber_callback, this); + trial_subscriber_ = n.subscribe("/gps_controller_trial_command", 1, &MobileRobot::trial_subscriber_callback, this); + data_request_subscriber_ = n.subscribe("/gps_controller_data_request", 1, &MobileRobot::data_request_subscriber_callback, this); + + // Create publishers. + report_publisher_.reset(new realtime_tools::RealtimePublisher(n, "/gps_controller_report", 1)); +} + +// Initialize all sensors. +void MobileRobot::initialize_sensors(ros::NodeHandle& n) +{ + ROS_INFO_STREAM("Initializing Tutlebot sensor"); + // Clear out the old sensors. + sensors_.clear(); + + // Create all sensors. + // NOTE: gps::TRIAL_ARM to only initial sensors_ (get rid of auxiliary sensor) + // TODO: Create image sensor here. + int i = MobileSensorType; + ROS_INFO_STREAM("creating sensor: " + to_string(i)); + boost::shared_ptr sensor(Sensor::create_sensor((SensorType)i,n,this,gps::TRIAL_ARM)); + sensors_.push_back(sensor); + + // Create current state sample and populate it using the sensors. + current_time_step_sample_.reset(new Sample(MAX_TRIAL_LENGTH)); + + // initialize sample + initialize_sample(current_time_step_sample_, gps::TRIAL_ARM); + + sensors_initialized_ = true; +} + +// Initialize position controllers. +void MobileRobot::initialize_position_controllers(ros::NodeHandle& n) +{ + nav_controller_.reset(new NavController(n)); +} + +// Update the controllers at each time step. +void MobileRobot::update_controllers(ros::Time current_time, bool is_controller_step) +{ + bool trial_init = trial_controller_ != NULL && trial_controller_->is_configured() && controller_initialized_; + if(!is_controller_step && trial_init){ + return; + } + + // If we have a trial controller, update that, otherwise update position controller. + // TODO: Uncomment if implemented navigation controller + if (trial_init) trial_controller_->update(this, current_time, current_time_step_sample_, active_arm_torques_); + else nav_controller_->update(this, current_time, current_time_step_sample_, active_arm_torques_); + + // Check if the trial controller finished and delete it. + if (trial_init && trial_controller_->is_finished()) { + + // Publish sample after trial completion + publish_sample_report(current_time_step_sample_, trial_controller_->get_trial_length()); + //Clear the trial controller. + trial_controller_->reset(current_time); + trial_controller_.reset(NULL); + + // Set the active arm controller to NO_CONTROL. + // TODO: Uncomment if implemented navigation controller + /*OptionsMap options; + options["mode"] = gps::NO_CONTROL; + active_arm_controller_->configure_controller(options);*/ + + + // Switch the sensors to run at full frequency. + for (int sensor = 0; sensor < TotalSensorTypes; sensor++) + { + //sensors_[sensor]->set_update(active_arm_controller_->get_update_delay()); + } + } + + // TODO: Uncomment if implemented navigation controller + if (nav_controller_->report_waiting){ + if (nav_controller_->is_finished()){ + publish_sample_report(current_time_step_sample_); + nav_controller_->report_waiting = false; + } + } +} + +// Position command callback. +void MobileRobot::nav_subscriber_callback(const gps_agent_pkg::NavigationCommand::ConstPtr& msg) +{ + ROS_INFO_STREAM("received navigation command"); + OptionsMap params; + + Eigen::VectorXd position; + position.resize(msg->position.size()); + for(int i=0; iposition[i]; + } + params["position"] = position; + + Eigen::VectorXd orientation; + + orientation.resize(msg->quaternion.size()); + for(int i=0; iquaternion[i]; + } + params["orientation"] = orientation; + + nav_controller_->configure_controller(params); +} + +// Trial command callback. +void MobileRobot::trial_subscriber_callback(const gps_agent_pkg::TrialCommand::ConstPtr& msg) +{ + RobotPlugin::trial_subscriber_callback(msg); +} + +// Trial command callback. +void MobileRobot::data_request_subscriber_callback(const gps_agent_pkg::DataRequest::ConstPtr& msg) +{ + RobotPlugin::data_request_subscriber_callback(msg); +} + +// Get current time. +ros::Time MobileRobot::get_current_time() const +{ + return last_update_time_; +} + +// Get current encoder readings (robot-dependent). +void MobileRobot::get_joint_encoder_readings(Eigen::VectorXd &angles, gps::ActuatorType arm) const +{ +} diff --git a/src/gps_agent_pkg/src/mobilerobot_node.cpp b/src/gps_agent_pkg/src/mobilerobot_node.cpp new file mode 100644 index 000000000..407d67688 --- /dev/null +++ b/src/gps_agent_pkg/src/mobilerobot_node.cpp @@ -0,0 +1,37 @@ +/* + * mobilerobot_node.cpp + * + * Created on: Mar 5, 2017 + * Author: thobotics + */ +#include "gps_agent_pkg/mobilerobot.h" +using namespace gps_control; + +int main(int argc, char** argv){ + ros::init(argc, argv, "mobilerobot_node"); + ros::NodeHandle n; + + // NOTE: Rate depend on simulator time + // E.g: stage_ros real-time is 10Hz (/clock topic) + ros::Rate rate(20); + + MobileRobot mobilerobot; + mobilerobot.init(n); + mobilerobot.starting(); + + // Control Loop and code + while (ros::ok()) + { + mobilerobot.update(); + ros::spinOnce(); + rate.sleep(); + } + + // It should never be called + mobilerobot.stopping(); + + return(0); +} + + + diff --git a/src/gps_agent_pkg/src/mobilerobotsensor.cpp b/src/gps_agent_pkg/src/mobilerobotsensor.cpp new file mode 100644 index 000000000..735646e8a --- /dev/null +++ b/src/gps_agent_pkg/src/mobilerobotsensor.cpp @@ -0,0 +1,486 @@ +/* + * mobilerobotsensor.cpp + * + * Created on: Mar 8, 2017 + * Author: thobotics + */ + +#include "gps_agent_pkg/mobilerobotsensor.h" + +#include "../include/gps_agent_pkg/mobilerobot.h" + +using namespace gps_control; + +char* MobileRobotSensor::cost_translation_table_ = NULL; + +// Constructor. +MobileRobotSensor::MobileRobotSensor(ros::NodeHandle& n, RobotPlugin *plugin): Sensor(n, plugin) +{ + // Initialize pose. + position_.resize(3); + orientation_.resize(4); // Quaternion + + // Initialize velocities + linear_velocities_.resize(3); + angular_velocities_.resize(3); + + // Initial position of nearest obstacle. + nearest_obstacle_.resize(3); + + // TODO: using parameter + range_data_.resize(30); + + potential_score_.resize(2); + + // Initialize cost map + tf_ = new tf::TransformListener(ros::Duration(10)); + costmap_ros_ = new costmap_2d::Costmap2DROS("local_costmap", *tf_); + costmap_ros_->start(); + + // For compute obstacle tree + // NOTE: Copy from costmap_2d_publisher. + data = NULL; + obs_tree = NULL; + if (cost_translation_table_ == NULL) + { + cost_translation_table_ = new char[256]; + + // special values: + cost_translation_table_[0] = 0; // NO obstacle + cost_translation_table_[253] = 99; // INSCRIBED obstacle + cost_translation_table_[254] = 100; // LETHAL obstacle + cost_translation_table_[255] = -1; // UNKNOWN + + // regular cost values scale the range 1 to 252 (inclusive) to fit + // into 1 to 98 (inclusive). + for (int i = 1; i < 253; i++) + { + cost_translation_table_[ i ] = char(1 + (97 * (i - 1)) / 251); + } + } + + // Initialize subscriber + n.param("/odom_topic", topic_name_, "odom"); + n.param("/scan_topic", range_topic_name_, "scan"); + n.param("/navfn_topic", potential_topic_name_, "/globalplanner_node/GlobalPlanner/potential"); + + subscriber_ = n.subscribe(topic_name_, 1, &MobileRobotSensor::update_data_vector, this); + range_subscriber_ = n.subscribe(range_topic_name_, 1, &MobileRobotSensor::update_range_data, this); + navfn_subscriber_ = n.subscribe (potential_topic_name_, 1, &MobileRobotSensor::update_navfn, this); + nearest_obs_pub_ = n.advertise("nearest_obstacle", 10); + + // Set time. + previous_pose_time_ = ros::Time(0.0); // This ignores the velocities on the first step. +} + +// Destructor. +MobileRobotSensor::~MobileRobotSensor() +{ + // Nothing to do here. +} + +// Update the sensor (called every tick). +void MobileRobotSensor::update(RobotPlugin *plugin, ros::Time current_time, bool is_controller_step) +{ + if (is_controller_step) + { + // Must call this before finding the closest obstacle + updateObstacleTree(costmap_ros_->getCostmap()); + + // TODO: Update current robot pose, velocities + double update_time = current_time.toSec() - previous_pose_time_.toSec(); + // ... + // Update stored time. + + // Find the nearest obstacle, default is -1000.0, -1000.0 + double obstacle_heading = 0.0; + Point obs_pose(-1000.0,-1000.0); + double minDist; + + { + boost::mutex::scoped_lock lock(odom_mutex_); + minDist = min_distance_to_obstacle(cur_pose_, &obstacle_heading, &obs_pose); + potential_score_[0] = getGlobalPointPotential(cur_pose_); + } + + nearest_obstacle_[0] = obs_pose.a; + nearest_obstacle_[1] = obs_pose.b; + nearest_obstacle_[2] = 0.0; + + visualization_msgs::Marker nearest_points; + nearest_points.header.frame_id = "odom"; + nearest_points.header.stamp = ros::Time::now(); + nearest_points.ns = "nearest_points"; + nearest_points.action = visualization_msgs::Marker::MODIFY; + nearest_points.pose.orientation.w = 1; + nearest_points.id = 0; + nearest_points.type = visualization_msgs::Marker::POINTS; + nearest_points.scale.x = 0.2; + nearest_points.scale.y = 0.2; + + // Points are green + nearest_points.color.g = 1.0f; + nearest_points.color.a = 1.0; + + geometry_msgs::Point p; + p.x = obs_pose.a; + p.y = obs_pose.b; + p.z = 1; + + nearest_points.points.push_back(p); + nearest_obs_pub_.publish(nearest_points); + + ROS_INFO("Cur pose %.2f, %.2f", cur_pose_.position.x, cur_pose_.position.y); + ROS_INFO("Min distance: %.2f, Pot: %.2f, Pose: %.2f, %.2f", minDist, potential_score_[0], obs_pose.a, obs_pose.b); + + previous_pose_time_ = current_time; + } +} + +void MobileRobotSensor::configure_sensor(OptionsMap &options) +{ + ROS_INFO("configuring mobilerobotsensor"); +} + +// Set data format and meta data on the provided sample. +void MobileRobotSensor::set_sample_data_format(boost::scoped_ptr& sample) +{ + // Set position size and format. + OptionsMap position_metadata; + sample->set_meta_data(gps::MOBILE_POSITION,position_.size(),SampleDataFormatEigenVector,position_metadata); + + // Set orientation size and format. + OptionsMap orientation_metadata; + sample->set_meta_data(gps::MOBILE_ORIENTATION,orientation_.size(),SampleDataFormatEigenVector,orientation_metadata); + + // Set nearest obstacle size and format. + OptionsMap nearest_obstacle_metadata; + sample->set_meta_data(gps::POSITION_NEAREST_OBSTACLE,nearest_obstacle_.size(),SampleDataFormatEigenVector,nearest_obstacle_metadata); + + // Set nearest obstacle size and format. + OptionsMap potential_score_metadata; + sample->set_meta_data(gps::POTENTIAL_SCORE,potential_score_.size(),SampleDataFormatEigenVector,potential_score_metadata); + + // Set linear velocities size and format. + OptionsMap linear_velocities_metadata; + sample->set_meta_data(gps::MOBILE_VELOCITIES_LINEAR,linear_velocities_.size(),SampleDataFormatEigenVector,linear_velocities_metadata); + + // Set angular velocities size and format. + OptionsMap angular_velocities_metadata; + sample->set_meta_data(gps::MOBILE_VELOCITIES_ANGULAR,angular_velocities_.size(),SampleDataFormatEigenVector,angular_velocities_metadata); + + // Set range data size and format. + OptionsMap range_metadata; + sample->set_meta_data(gps::MOBILE_RANGE_SENSOR,range_data_.size(),SampleDataFormatEigenVector,range_metadata); + + +} + +// Set data on the provided sample. +void MobileRobotSensor::set_sample_data(boost::scoped_ptr& sample, int t) +{ + { + boost::mutex::scoped_lock lock(odom_mutex_); + + // Set position. + sample->set_data_vector(t,gps::MOBILE_POSITION,position_.data(),position_.size(),SampleDataFormatEigenVector); + + // Set orientation. + sample->set_data_vector(t,gps::MOBILE_ORIENTATION,orientation_.data(),orientation_.size(),SampleDataFormatEigenVector); + + // Set linear velocities. + sample->set_data_vector(t,gps::MOBILE_VELOCITIES_LINEAR,linear_velocities_.data(),linear_velocities_.size(),SampleDataFormatEigenVector); + + // Set angular velocities. + sample->set_data_vector(t,gps::MOBILE_VELOCITIES_ANGULAR,angular_velocities_.data(),angular_velocities_.size(),SampleDataFormatEigenVector); + } + + // Set nearest obstacle. + sample->set_data_vector(t,gps::POSITION_NEAREST_OBSTACLE,nearest_obstacle_.data(),nearest_obstacle_.size(),SampleDataFormatEigenVector); + + sample->set_data_vector(t,gps::POTENTIAL_SCORE,potential_score_.data(),potential_score_.size(),SampleDataFormatEigenVector); + + { + boost::mutex::scoped_lock lock(range_mutex_); + // Set range data. + sample->set_data_vector(t,gps::MOBILE_RANGE_SENSOR,range_data_.data(),range_data_.size(),SampleDataFormatEigenVector); + } +} + +void MobileRobotSensor::update_data_vector(const nav_msgs::Odometry::ConstPtr& msg) +{ + { + boost::mutex::scoped_lock lock(odom_mutex_); + + // Update current robot pose and velocites + position_[0] = msg->pose.pose.position.x; + position_[1] = msg->pose.pose.position.y; + position_[2] = msg->pose.pose.position.z; + + orientation_[0] = msg->pose.pose.orientation.x; + orientation_[1] = msg->pose.pose.orientation.y; + orientation_[2] = msg->pose.pose.orientation.z; + orientation_[3] = msg->pose.pose.orientation.w; + + linear_velocities_[0] = msg->twist.twist.linear.x; + linear_velocities_[1] = msg->twist.twist.linear.y; + linear_velocities_[2] = msg->twist.twist.linear.z; + + angular_velocities_[0] = msg->twist.twist.angular.x; + angular_velocities_[1] = msg->twist.twist.angular.y; + angular_velocities_[2] = msg->twist.twist.angular.z; + + cur_pose_.position.x = position_[0]; + cur_pose_.position.y = position_[1]; + cur_pose_.position.z = position_[2]; + + cur_pose_.orientation.x = orientation_[0]; + cur_pose_.orientation.y = orientation_[1]; + cur_pose_.orientation.z = orientation_[2]; + cur_pose_.orientation.w = orientation_[3]; + } +} + +void MobileRobotSensor::update_range_data(const sensor_msgs::LaserScan::ConstPtr& msg) +{ + { + boost::mutex::scoped_lock lock(range_mutex_); + for(int i=0; i < range_data_.size(); i++) + { + range_data_[i] = msg->ranges[i]; + } + } +} + +void MobileRobotSensor::update_navfn(const nav_msgs::OccupancyGrid::ConstPtr& nav_cost) +{ + { + boost::mutex::scoped_lock lock(global_pot_mutex_); + + global_potarr_ = nav_cost->data; + global_width_ = nav_cost->info.width; + global_height_ = nav_cost->info.height; + origin_x_ = nav_cost->info.origin.position.x; + origin_y_ = nav_cost->info.origin.position.y; + resolution_ = nav_cost->info.resolution; + } +} + +geometry_msgs::Point MobileRobotSensor::transformOdomToMap(geometry_msgs::Pose local_pose){ + // Transform to global_pose + //clock_t begin_time = clock(); + geometry_msgs::PointStamped local_point; + local_point.header.frame_id = costmap_ros_->getGlobalFrameID(); + // This is important!!! + // It make transformPose to lookup the latest available transform + local_point.header.stamp = ros::Time(0); + local_point.point = local_pose.position; + geometry_msgs::PointStamped global_point_stamp; + try{ + tf_->waitForTransform("/map", costmap_ros_->getGlobalFrameID(), ros::Time(0), ros::Duration(10.0)); + tf_->transformPoint("/map", local_point, global_point_stamp); + }catch (tf::TransformException & ex){ + ROS_ERROR("Transform exception 111 : %s", ex.what()); + } + //ROS_INFO("Transform plan take %f", float( clock() - begin_time ) / CLOCKS_PER_SEC); + return global_point_stamp.point; +} + +double MobileRobotSensor::getGlobalPointPotential(geometry_msgs::Pose local_pose){ + //clock_t begin_time = clock(); + // Uninitialized + if (global_width_ == 0 || global_height_ == 0) return DBL_MAX; + geometry_msgs::Point currentPoint = transformOdomToMap(local_pose); + + double score; + { + boost::mutex::scoped_lock lock(global_pot_mutex_); + + // TODO: Interpolate here + bool flag = false; + unsigned int mx, my; + // Copy from worldToMap from Costmap2D + if (currentPoint.x < origin_x_ || currentPoint.y < origin_y_) + flag = false; + mx = (int)((currentPoint.x - origin_x_) / resolution_); + my = (int)((currentPoint.y - origin_y_) / resolution_); + if (mx < global_width_ && my < global_height_) + flag = true; + + //ROS_INFO("Get point potential take %f", float( clock() - begin_time ) / CLOCKS_PER_SEC); + + if(!flag) + score = DBL_MAX; + + unsigned int index = my * global_width_ + mx; + + if(global_potarr_[index] == -1){ + score = 100; // Highest + }else + score = global_potarr_[index]; + } + + return score; +} + +void MobileRobotSensor::updateObstacleTree(costmap_2d::Costmap2D *costmap) +{ + // Create occupancy grid message + // Copy from costmap_2d_publisher.cpp + nav_msgs::OccupancyGrid::_info_type::_resolution_type resolution = costmap->getResolution(); + nav_msgs::OccupancyGrid::_info_type::_width_type width = costmap->getSizeInCellsX(); + nav_msgs::OccupancyGrid::_info_type::_height_type height = costmap->getSizeInCellsY(); + double wx, wy; + costmap->mapToWorld(0, 0, wx, wy); + nav_msgs::OccupancyGrid::_info_type::_origin_type::_position_type::_x_type x = wx - resolution / 2; + nav_msgs::OccupancyGrid::_info_type::_origin_type::_position_type::_y_type y = wy - resolution / 2; + nav_msgs::OccupancyGrid::_info_type::_origin_type::_position_type::_z_type z = 0.0; + nav_msgs::OccupancyGrid::_info_type::_origin_type::_orientation_type::_w_type w = 1.0; + + nav_msgs::OccupancyGrid::_data_type grid_data; + grid_data.resize(width * height); + + unsigned char* charData = costmap->getCharMap(); + for (unsigned int i = 0; i < grid_data.size(); i++) + { + grid_data[i] = cost_translation_table_[ charData[ i ]]; + } + + // Copy from costmap_translator + nav_msgs::GridCells obstacles; + obstacles.cell_width = resolution; + obstacles.cell_height = resolution; + for (unsigned int i = 0 ; i < height; ++i) + { + for(unsigned int j = 0; j < width; ++j) + { + if(grid_data[i*height+j] == 100) + { + geometry_msgs::Point obstacle_coordinates; + obstacle_coordinates.x = (j * obstacles.cell_height) + x + (resolution/2.0); + obstacle_coordinates.y = (i * obstacles.cell_width) + y + (resolution/2.0); + obstacle_coordinates.z = 0; + obstacles.cells.push_back(obstacle_coordinates); + } + } + } + + // Copy from nav_cb + cost_map = obstacles; + + if(cost_map.cells.size() > 0) + { + delete obs_tree; + delete data; + data = new flann::Matrix(new float[obstacles.cells.size()*2], obstacles.cells.size(), 2); + + for (size_t i = 0; i < data->rows; ++i) + { + for (size_t j = 0; j < data->cols; ++j) + { + if (j == 0) + (*data)[i][j] = cost_map.cells[i].x; + else + (*data)[i][j] = cost_map.cells[i].y; + } + } + // Obstacle index for fast nearest neighbor search + obs_tree = new flann::Index >(*data, flann::KDTreeIndexParams(4)); + obs_tree->buildIndex(); + } +} + +MinDistResult MobileRobotSensor::find_nearest_neighbor(Point queryPoint) +{ + MinDistResult results; + + flann::Matrix query(new float[2], 1, 2); + query[0][0] = queryPoint.a; + query[0][1] = queryPoint.b; + + std::vector< std::vector > indices; + std::vector< std::vector > dists; + + flann::SearchParams params; + params.checks = 128; + params.sorted = true; + + { + boost::mutex::scoped_lock lock(cost_map_mutex_); + obs_tree->knnSearch(query, indices, dists, 1, params); + results.p = Point((*data)[indices[0][0]][0], (*data)[indices[0][0]][1]); + results.dist = static_cast(dists[0][0]); + } + + MinDistResult tempResults; + tempResults.p = Point(cost_map.cells[indices[0][0]].x, cost_map.cells[indices[0][0]].y); + + delete[] query.ptr(); + indices.clear(); + dists.clear(); + + return results; +} + +double MobileRobotSensor::min_distance_to_obstacle(geometry_msgs::Pose local_current_pose, double *heading, Point *obs_pose) +{ + if(cost_map.cells.size() == 0){ + return 100000; + } + // ROS_INFO("In minDist Function"); + Point global(local_current_pose.position.x, local_current_pose.position.y); + MinDistResult nn_graph_point = find_nearest_neighbor(global); + + double minDist = 100000; + double head = 0; + + minDist = distance(local_current_pose.position.x, local_current_pose.position.y, nn_graph_point.p.a, nn_graph_point.p.b); + + obs_pose->a = nn_graph_point.p.a; + obs_pose->b = nn_graph_point.p.b; + *heading = head; + + return minDist; +} + +double MobileRobotSensor::distance(double pose_x, double pose_y, double obx, double oby) +{ + double diffx = obx - pose_x; + double diffy = oby - pose_y; + double dist = sqrt(diffx*diffx + diffy*diffy); + return dist; +} + +double MobileRobotSensor::mod(double x, double y) +{ + double m= x - y * floor(x/y); + // handle boundary cases resulted from floating-point cut off: + if (y > 0) // modulo range: [0..y) + { + if (m >= y) // Mod(-1e-16 , 360. ): m= 360. + return 0; + + if (m < 0) + { + if (y+m == y) + return 0; // just in case... + else + return y+m; // Mod(106.81415022205296 , _TWO_PI ): m= -1.421e-14 + } + } + else // modulo range: (y..0] + { + if (m <= y) // Mod(1e-16 , -360. ): m= -360. + return 0; + + if (m>0 ) + { + if (y+m == y) + return 0; // just in case... + else + return y+m; // Mod(-106.81415022205296, -_TWO_PI): m= 1.421e-14 + } + } + + return m; +} diff --git a/src/gps_agent_pkg/src/mpepccontrollaw.cpp b/src/gps_agent_pkg/src/mpepccontrollaw.cpp new file mode 100644 index 000000000..8be7a6f1d --- /dev/null +++ b/src/gps_agent_pkg/src/mpepccontrollaw.cpp @@ -0,0 +1,192 @@ +/* + * control_law.cpp + * + * Created on: Dec 2, 2016 + * Author: thobotics + */ + +#include + +#include "gps_agent_pkg/mpepccontrollaw.h" + +namespace mpepc_local_planner +{ + ControlLaw::ControlLaw() + { + } + + ControlLaw::ControlLaw(ControlLawSettings* c) + { + settings_ = c; + } + + // Floating-point modulo + // The result (the remainder) has same sign as the divisor. + // Similar to matlab's mod(); Not similar to fmod() - Mod(-3,4)= 1 fmod(-3,4)= -3 + double ControlLaw::mod(double x, double y) + { + double m= x - y * floor(x/y); + // handle boundary cases resulted from floating-point cut off: + if (y > 0) // modulo range: [0..y) + { + if (m >= y) // Mod(-1e-16 , 360. ): m= 360. + return 0; + + if (m < 0) + { + if (y+m == y) + return 0; // just in case... + else + return y+m; // Mod(106.81415022205296 , _TWO_PI ): m= -1.421e-14 + } + } + else // modulo range: (y..0] + { + if (m <= y) // Mod(1e-16 , -360. ): m= -360. + return 0; + + if (m > 0) + { + if (y+m == y) + return 0; // just in case... + else + return y+m; // Mod(-106.81415022205296, -_TWO_PI): m= 1.421e-14 + } + } + return m; + } + + void ControlLaw::update_k1_k2(double k1, double k2) + { + settings_->m_K1 = k1; + settings_->m_K2 = k2; + } + + // wrap [rad] angle to [-PI..PI) + double ControlLaw::wrap_pos_neg_pi(double angle) + { + return mod(angle + _PI, _TWO_PI) - _PI; + } + + double ControlLaw::get_ego_distance(geometry_msgs::Pose current_pose, geometry_msgs::Pose current_goal_pose) + { + double distance; + + double dx = current_goal_pose.position.x - current_pose.position.x; + double dy = current_goal_pose.position.y - current_pose.position.y; + + // calculate distance compenent of egocentric coordinates + distance = sqrt(pow(dx, 2) + pow(dy, 2)); + + return distance; + } + + + EgoPolar ControlLaw::convert_to_egopolar(geometry_msgs::Pose current_pose, geometry_msgs::Pose current_goal_pose) + { + EgoPolar coords; + + double dx = current_goal_pose.position.x - current_pose.position.x; + double dy = current_goal_pose.position.y - current_pose.position.y; + double obs_heading = atan2(dy, dx); + double current_yaw = tf::getYaw(current_pose.orientation); + double goal_yaw = tf::getYaw(current_goal_pose.orientation); + + // calculate r + coords.r = sqrt(pow(dx, 2) + pow(dy, 2)); + // calculate delta + coords.delta = wrap_pos_neg_pi(current_yaw - obs_heading); + // calculate theta + coords.theta = wrap_pos_neg_pi(goal_yaw - obs_heading); + + return coords; + } + + geometry_msgs::Pose ControlLaw::convert_from_egopolar(geometry_msgs::Pose current_pose, EgoPolar current_goal_coords) + { + geometry_msgs::Pose current_goal_pose; + + double current_yaw = tf::getYaw(current_pose.orientation); + + current_goal_pose.position.x = current_pose.position.x + current_goal_coords.r * cos(current_yaw - current_goal_coords.delta); + current_goal_pose.position.y = current_pose.position.y + current_goal_coords.r * sin(current_yaw - current_goal_coords.delta); + current_goal_pose.position.z = 0; + + current_goal_pose.orientation = tf::createQuaternionMsgFromYaw(current_yaw - current_goal_coords.delta + current_goal_coords.theta); + + return current_goal_pose; + } + + double ControlLaw::get_kappa(EgoPolar current_ego_goal, double k1, double k2) + { + double kappa = 0; + kappa = (-1/current_ego_goal.r)*(k2*(current_ego_goal.delta-atan(-1*k1*current_ego_goal.theta)) + (1 + k1/(1+pow(k1*current_ego_goal.theta, 2)))*sin(current_ego_goal.delta)); + return kappa; + } + + double ControlLaw::get_linear_vel(double kappa, EgoPolar current_ego_goal, double vMax) + { + double lin_vel = 0; + vMax = settings_->m_V_MAX; + lin_vel = min(vMax/settings_->m_R_THRESH*current_ego_goal.r, vMax/(1 + settings_->m_BETA * pow(abs(kappa), settings_->m_LAMBDA))); + + if (lin_vel < settings_->m_V_MIN && lin_vel > 0.00) + { + lin_vel = settings_->m_V_MIN; + } + return lin_vel; + } + + double ControlLaw::calc_sigmoid(double time_tau) + { + double sigma = 0; + sigma = 1.02040816*(1/(1+exp(-9.2*(time_tau-0.5))) - 0.01); + if (sigma > 1) + sigma = 1; + else if (sigma < 0) + sigma = 0; + return sigma; + } + + geometry_msgs::Twist ControlLaw::get_velocity_command(geometry_msgs::Pose current_position, geometry_msgs::Pose goal, double k1, double k2, double vMax) + { + EgoPolar goal_coords = convert_to_egopolar(current_position, goal); + + return get_velocity_command(goal_coords, k1, k2, vMax); + } + + geometry_msgs::Twist ControlLaw::get_velocity_command(EgoPolar goal_coords, double vMax) + { + return get_velocity_command(goal_coords, settings_->m_K1, settings_->m_K2, vMax); + } + + geometry_msgs::Twist ControlLaw::get_velocity_command(EgoPolar goal_coords, double k1, double k2, double vMax) + { + geometry_msgs::Twist cmd_vel; + double kappa = 0; + + kappa = get_kappa(goal_coords, k1, k2); + + cmd_vel.linear.x = get_linear_vel(kappa, goal_coords, vMax); + cmd_vel.angular.z = kappa*cmd_vel.linear.x; + if (fabs(cmd_vel.angular.z) > R_SPEED_LIMIT) + { + if (cmd_vel.angular.z > 0) + { + cmd_vel.angular.z = R_SPEED_LIMIT; + } + else + { + cmd_vel.angular.z = -1*R_SPEED_LIMIT; + } + cmd_vel.linear.x = cmd_vel.angular.z/kappa; + } + + return cmd_vel; + } +} // end namespace + + + + + diff --git a/src/gps_agent_pkg/src/mpepcmobilerobot.cpp b/src/gps_agent_pkg/src/mpepcmobilerobot.cpp new file mode 100644 index 000000000..6897effee --- /dev/null +++ b/src/gps_agent_pkg/src/mpepcmobilerobot.cpp @@ -0,0 +1,67 @@ +/* + * mpepcmobilerobot.cpp + * + * Created on: Feb 20, 2018 + * Author: thobotics + */ +#include "gps_agent_pkg/mpepcmobilerobot.h" + +using namespace gps_control; + +// Plugin constructor. +MpepcMobileRobot::MpepcMobileRobot() +:MobileRobot() +{ + // Nothing to do here, since all variables are initialized in initialize(...) + // Some basic variable initialization. +} + +// Destructor. +MpepcMobileRobot::~MpepcMobileRobot() +{ + // Nothing to do here, since all instance variables are destructed automatically. +} + +void MpepcMobileRobot::init(ros::NodeHandle& n) +{ + // TODO: Set topic name by parameters + ego_goal_pub_ = n.advertise("/ego_goal", 1); + + + // EgoGoal r, delta, theta, vMax + active_arm_torques_.resize(4); + + // Initialize ROS subscribers/publishers, sensors, and navigation controllers. + initialize(n); +} + +void MpepcMobileRobot::update() +{ + /* + * This is the same as the original Mobile Robot */ + // Get current time. + last_update_time_ = ros::Time::now(); + + // Check if this is a controller step based on the current controller frequency. + controller_counter_++; + if (controller_counter_ >= controller_step_length_) controller_counter_ = 0; + bool is_controller_step = (controller_counter_ == 0); + + // Update the sensors and fill in the current step sample. + update_sensors(last_update_time_,is_controller_step); + + // Update the controllers. + update_controllers(last_update_time_,is_controller_step); + + /* + * Publish ego goal instead of raw v, w */ + // Setup action and send to robot + gps_agent_pkg::EgoGoal ego_goal; + ego_goal.r = active_arm_torques_[0]; + ego_goal.delta = active_arm_torques_[1]; + ego_goal.theta = active_arm_torques_[2]; + ego_goal.vMax = active_arm_torques_[3]; + ego_goal_pub_.publish(ego_goal); +} + + diff --git a/src/gps_agent_pkg/src/mpepcmobilerobot_node.cpp b/src/gps_agent_pkg/src/mpepcmobilerobot_node.cpp new file mode 100644 index 000000000..ad6ba3f11 --- /dev/null +++ b/src/gps_agent_pkg/src/mpepcmobilerobot_node.cpp @@ -0,0 +1,37 @@ +/* + * mobilerobot_node.cpp + * + * Created on: Mar 5, 2017 + * Author: thobotics + */ +#include "gps_agent_pkg/mpepcmobilerobot.h" +using namespace gps_control; + +int main(int argc, char** argv){ + ros::init(argc, argv, "mpepcmobilerobot_node"); + ros::NodeHandle n; + + // NOTE: Rate depend on simulator time + // E.g: stage_ros real-time is 10Hz (/clock topic) + ros::Rate rate(20); + + MpepcMobileRobot mobilerobot; + mobilerobot.init(n); + mobilerobot.starting(); + + // Control Loop and code + while (ros::ok()) + { + mobilerobot.update(); + ros::spinOnce(); + rate.sleep(); + } + + // It should never be called + mobilerobot.stopping(); + + return(0); +} + + + diff --git a/src/gps_agent_pkg/src/navcontroller.cpp b/src/gps_agent_pkg/src/navcontroller.cpp new file mode 100644 index 000000000..bf0c89005 --- /dev/null +++ b/src/gps_agent_pkg/src/navcontroller.cpp @@ -0,0 +1,77 @@ +/* + * navcontroller.cpp + * + * Created on: Mar 8, 2017 + * Author: thobotics + */ +#include "gps_agent_pkg/navcontroller.h" +#include "gps_agent_pkg/robotplugin.h" +#include "gps_agent_pkg/util.h" + +using namespace gps_control; + +// Constructor. +NavController::NavController(ros::NodeHandle& n) + : Controller(n, gps::TRIAL_ARM, 0) +{ + report_waiting = false; + // TODO: Change topic names to parameters or something + nav_pub_ = n.advertise("/cmd_pose", 1); +} + +// Destructor. +NavController::~NavController() +{ +} + +// Update the controller (take an action). +void NavController::update(RobotPlugin *plugin, ros::Time current_time, boost::scoped_ptr& sample, Eigen::VectorXd &torques) +{ + // TODO: Send target pose to move_base and waiting it finished + if(finished_){ + torques = Eigen::VectorXd::Zero(torques.rows()); + } +} + +// Configure the controller. +void NavController::configure_controller(OptionsMap &options) +{ + // This sets the target position. + ROS_INFO_STREAM("Received controller configuration"); + // needs to report when finished + report_waiting = true; + finished_ = false; + + // Update target pose of robot + position_ = boost::get(options["position"]); + orientation_ = boost::get(options["orientation"]); + + // NOTE: This pose for Stage frame (map). + // Its coordinate (yzx ??, x,y = 0.707) + // is not the same as odom (zyx, w,z = 0.707) + geometry_msgs::Pose stage_pose; + stage_pose.position.x = position_(0); + stage_pose.position.y = position_(1); + stage_pose.position.z = position_(2); + stage_pose.orientation.x = orientation_(0); + stage_pose.orientation.y = orientation_(1); + stage_pose.orientation.z = orientation_(2); + stage_pose.orientation.w = orientation_(3); + nav_pub_.publish(stage_pose); + + // TODO: It totally wrong, must check position in is_finished + finished_ = true; +} + +// Check if controller is finished with its current task. +bool NavController::is_finished() const +{ + return finished_; // TODO: CHECK Reach goal and return +} + +// Reset the controller -- this is typically called when the controller is turned on. +void NavController::reset(ros::Time time) +{ + // Clear update time. + // last_update_time_ = ros::Time(0.0); +} diff --git a/src/gps_agent_pkg/src/sensor.cpp b/src/gps_agent_pkg/src/sensor.cpp index f950d2c7a..dec77edf0 100755 --- a/src/gps_agent_pkg/src/sensor.cpp +++ b/src/gps_agent_pkg/src/sensor.cpp @@ -1,6 +1,7 @@ #include "gps_agent_pkg/sensor.h" #include "gps_agent_pkg/encodersensor.h" #include "gps_agent_pkg/rostopicsensor.h" +#include "gps_agent_pkg/mobilerobotsensor.h" using namespace gps_control; @@ -16,7 +17,9 @@ Sensor* Sensor::create_sensor(SensorType type, ros::NodeHandle& n, RobotPlugin * return CameraSensor(n,plugin); */ case ROSTopicSensorType: - return (Sensor *) (new ROSTopicSensor(n,plugin)); + return (Sensor *) (new ROSTopicSensor(n,plugin)); + case MobileSensorType: + return (Sensor *) (new MobileRobotSensor(n,plugin)); default: ROS_ERROR("Unknown sensor type %i requested from sensor constructor!",type); diff --git a/src/gps_agent_pkg/worlds/hallway.world b/src/gps_agent_pkg/worlds/hallway.world new file mode 100644 index 000000000..5e0cfd5a0 --- /dev/null +++ b/src/gps_agent_pkg/worlds/hallway.world @@ -0,0 +1,86 @@ +define kinect ranger +( + sensor + ( + range_max 6.5 + fov 180.0 + samples 30 + ) + # generic model properties + color "black" + size [ 0.060 0.150 0.030 ] +) + +define turtlebot position +( + pose [ 0.0 0.0 0.0 0.0 ] + + odom_error [0.03 0.03 999999 999999 999999 0.02] + + size [ 0.255 0.255 0.400 ] + origin [ 0.000 0.000 0.000 0.000 ] + gui_nose 1 + drive "diff" + color "gray" + + kinect(pose [ -0.100 0.000 -0.110 0.000 ]) + # add a camera to the robot + camera( + pose [ 0 0 0.2 0 ] + range [ 0.2 8.0 ] + resolution [ 100 100 ] + fov [ 70 40 ] + pantilt [ 0 0 ] + alwayson 1 + ) +) + +define floorplan model +( + # sombre, sensible, artistic + color "gray30" + + # most maps will need a bounding box + boundary 1 + + gui_nose 0 + gui_grid 0 + gui_outline 0 + gripper_return 0 + fiducial_return 0 + laser_return 1 +) + +define block model +( + size [0.75 0.75 0.500] + gui_nose 0 +) + +resolution 0.02 +interval_sim 100 # simulation timestep in milliseconds + +window +( + size [ 600 700 ] + center [ 11.007 9.344 ] + rotate [ 0.000 0.000 ] + scale 21.961 +) + +floorplan +( + name "hallway" + bitmap "../maps/hallway.png" + size [ 38.000 20.000 2.000 ] + pose [ 19.000 10.000 0.000 0.000 ] +) + +# throw in a robot +turtlebot +( + pose [ 3.5 10.3 0.000 0 ] + name "turtlebot" + color "black" +) +#block( pose [ 8.5 8.3 0.000 180.000 ] color "red") diff --git a/src/gps_agent_pkg/worlds/hallway_bend.world b/src/gps_agent_pkg/worlds/hallway_bend.world new file mode 100644 index 000000000..44cda55ba --- /dev/null +++ b/src/gps_agent_pkg/worlds/hallway_bend.world @@ -0,0 +1,89 @@ +define kinect ranger +( + sensor + ( + range_max 6.5 + fov 180.0 + samples 30 + ) + model + ( + # generic model properties + size [ 0.07 0.07 0.05 ] # dimensions from LMS200 data sheet + color "blue" + ) +) + +define turtlebot position +( + pose [ 0.0 0.0 0.0 0.0 ] + + #odom_error [0.03 0.03 999999 999999 999999 0.02] + + size [ 0.255 0.255 0.400 ] + origin [ 0.000 0.000 0.000 0.000 ] + gui_nose 1 + drive "diff" + color "gray" + + kinect(pose [ -0.100 0.000 -0.110 0.000 ]) + # add a camera to the robot + camera( + pose [ 0 0 0.2 0 ] + range [ 0.2 8.0 ] + resolution [ 100 100 ] + fov [ 70 40 ] + pantilt [ 0 0 ] + alwayson 1 + ) +) + +define floorplan model +( + # sombre, sensible, artistic + color "gray30" + + # most maps will need a bounding box + boundary 1 + + gui_nose 0 + gui_grid 0 + gui_outline 0 + gripper_return 0 + fiducial_return 0 + laser_return 1 +) + +define block model +( + size [0.75 0.75 0.500] + gui_nose 0 +) + +resolution 0.02 +interval_sim 100 # simulation timestep in milliseconds + +window +( + size [ 600 700 ] + center [ 11.007 9.344 ] + rotate [ 0.000 0.000 ] + scale 21.961 +) + +floorplan +( + name "hallway" + bitmap "../maps/hallway_bend.png" + size [ 38.000 20.000 2.000 ] + pose [ 19.000 10.000 0.000 0.000 ] +) + +# throw in a robot +turtlebot +( + pose [ 3.5 11.25 0.000 0 ] + name "turtlebot" + color "black" +) +#block( pose [ 8.5 8.3 0.000 180.000 ] color "red") diff --git a/src/gps_agent_pkg/worlds/one_obstacle.world b/src/gps_agent_pkg/worlds/one_obstacle.world new file mode 100644 index 000000000..7fb17b0bf --- /dev/null +++ b/src/gps_agent_pkg/worlds/one_obstacle.world @@ -0,0 +1,91 @@ +define kinect ranger +( + sensor + ( + range_max 6.5 + fov 180.0 + samples 30 + ) + # generic model properties + color "black" + size [ 0.060 0.150 0.030 ] +) + +define turtlebot position +( + pose [ 0.0 0.0 0.0 0.0 ] + + odom_error [0.03 0.03 999999 999999 999999 0.02] + + size [ 0.255 0.255 0.400 ] + origin [ 0.000 0.000 0.000 0.000 ] + gui_nose 1 + drive "omni" + color "gray" + + kinect(pose [ -0.100 0.000 -0.110 0.000 ]) +) + +define floorplan model +( + # sombre, sensible, artistic + color "gray30" + + # most maps will need a bounding box + boundary 1 + + gui_nose 0 + gui_grid 0 + gui_outline 0 + gripper_return 0 + fiducial_return 0 + laser_return 1 +) + +define block model +( + size [1.0 1.0 1.500] + gui_nose 0 +) + +define block2 model +( + size [1.0 1.0 1.500] + gui_nose 0 +) + +define block3 model +( + size [1.0 1.0 1.500] + gui_nose 0 +) + +resolution 0.02 +interval_sim 100 # simulation timestep in milliseconds + +window +( + size [ 600 700 ] + center [ 11.007 9.344 ] + rotate [ 0.000 0.000 ] + scale 21.961 +) + +floorplan +( + name "one_obstacle" + bitmap "../maps/dynamic_obstacle_ext.png" + size [ 35.000 20.000 2.000 ] + pose [ 17.500 10.000 0.000 0.000 ] +) + +# throw in a robot +turtlebot +( + pose [ 1.5 8.3 0.000 0 ] + name "turtlebot" + color "black" +) +block( pose [ 7.5 8.3 0.000 180.000 ] color "red") +block2( pose [ 11.9 10.9 0.000 180.000 ] color "red") +block3( pose [ 17.7 9.2 0.000 180.000 ] color "red") diff --git a/src/proto/gps.proto b/src/proto/gps.proto index 4627cf547..5bf9faf5d 100755 --- a/src/proto/gps.proto +++ b/src/proto/gps.proto @@ -23,8 +23,15 @@ enum SampleType { IMAGE_FEAT = 16; END_EFFECTOR_POINTS_NO_TARGET = 17; END_EFFECTOR_POINT_VELOCITIES_NO_TARGET = 18; - NOISE = 19; - TOTAL_DATA_TYPES = 20; + POSITION_NEAREST_OBSTACLE = 19; + MOBILE_POSITION = 20; + MOBILE_ORIENTATION = 21; + MOBILE_VELOCITIES_LINEAR = 22; + MOBILE_VELOCITIES_ANGULAR = 23; + MOBILE_RANGE_SENSOR = 24; + POTENTIAL_SCORE = 25; + NOISE = 26; + TOTAL_DATA_TYPES = 27; } // Message containing the data for a single sample.