def __init__(self, dof, sig, num_dyn_factors, batch_size=1, use_cuda=False):
self.use_cuda = torch.cuda.is_available() if use_cuda else False
self.device = torch.device('cuda') if self.use_cuda else torch.device('cpu')
self.dof = dof
self.num_dyn_factors = num_dyn_factors
self.cov = mat_utils.isotropic_matrix(torch.pow(sig,2.0), 1, self.device).unsqueeze(0).repeat(self.num_dyn_factors,1,1)
self.inv_cov = mat_utils.isotropic_matrix(1.0/torch.pow(sig,2.0), 1,self.device).unsqueeze(0).repeat(self.num_dyn_factors,1,1)
def __init__(self, ndims, sig, batch_size=1, use_cuda=False):
self.use_cuda = torch.cuda.is_available() if use_cuda else False
self.device = torch.device('cuda') if self.use_cuda else torch.device(
'cpu')
self.ndims = torch.tensor(ndims, device=self.device)
self.cov = mat_utils.isotropic_matrix(torch.pow(sig, 2.0), self.ndims,
self.device)
def __init__(self,
ndims,
num_vel_factors,
sig,
batch_size=1,
use_cuda=False):
self.use_cuda = torch.cuda.is_available() if use_cuda else False
self.device = torch.device('cuda') if self.use_cuda else torch.device(
'cpu')
self.num_vel_factors = num_vel_factors
self.ndims = torch.tensor(ndims, device=self.device)
self.cov = mat_utils.isotropic_matrix(torch.pow(sig, 2.0), self.ndims,
self.device)
self.batch_size = batch_size
self.inv_cov = mat_utils.isotropic_matrix(
1.0 / torch.pow(sig, 2.0), self.ndims / 2,
self.device).unsqueeze(0).repeat(self.num_vel_factors, 1, 1)
def __init__(self,
gp_params,
obs_params,
planner_params,
env_params,
robot,
use_cuda=False):
"""
The GPMP2 Planner class
"""
self.use_cuda = torch.cuda.is_available() if use_cuda else False
self.device = torch.device('cuda') if self.use_cuda else torch.device(
'cpu')
# self.env = env
self.robot = robot
self.gp_params = gp_params
self.obs_params = obs_params
self.env_params = env_params
self.dof = planner_params['dof']
self.state_dim = planner_params['state_dim']
self.total_time_sec = planner_params['total_time_sec']
self.total_time_step = planner_params[
'total_time_step'] #Number of states in the trajectory
self.total_check_step = planner_params['total_check_step']
self.use_gp_inter = planner_params['use_gp_inter']
self.num_traj_states = self.total_time_step + 1 #+1 for goal at the end
self.dt = self.total_time_sec * 1.0 / self.total_time_step * 1.0
self.check_inter = (self.total_check_step / self.total_time_step) - 1.0
#Dimensions of the linear system
self.num_gp_factors = self.num_traj_states - 1
self.num_prior_factors = 2 #Prior on start and goal
if self.use_gp_inter:
self.num_obs_factors = self.num_traj_states + self.num_traj_states * self.total_check_step
else:
self.num_obs_factors = self.num_traj_states
self.nlinks = self.robot.nlinks
self.M = self.state_dim * (self.num_gp_factors + self.num_prior_factors
) + self.num_obs_factors * self.nlinks
self.N = self.state_dim * self.num_traj_states
qc_inv = self.gp_params['Q_c_inv']
qc_inv_traj = torch.zeros(self.num_gp_factors,
self.dof,
self.dof,
device=self.device)
qc_inv_traj = qc_inv_traj + qc_inv
inv_cov = mat_utils.isotropic_matrix(
1.0 / torch.pow(self.obs_params['cost_sigma'], 2.0),
self.robot.nlinks, self.device)
inv_obscov_traj = torch.zeros(self.num_traj_states,
self.robot.nlinks,
self.robot.nlinks,
device=self.device)
inv_obscov_traj = inv_obscov_traj + inv_cov
self.gp_prior = GPFactor(self.dof, self.dt, self.num_gp_factors,
self.use_cuda)
self.start_prior = PriorFactor(self.state_dim, self.gp_params['K_s'])
self.goal_prior = PriorFactor(self.state_dim, self.gp_params['K_g'])
self.obs_factor = ObstacleFactor(self.state_dim, self.num_obs_factors,
self.obs_params['epsilon_dist'],
self.env_params, self.robot,
self.use_cuda)
self.gp_prior.set_Q_c_inv(qc_inv_traj)
self.obs_factor.set_inv_cov(inv_obscov_traj)
def __init__(self,
gp_params,
obs_params,
planner_params,
optim_params,
env_params,
robot_model,
learn_params=None,
batch_size=1,
use_cuda=False):
super(PlanLayer, self).__init__()
self.use_cuda = torch.cuda.is_available() if use_cuda else False
self.device = torch.device('cuda') if self.use_cuda else torch.device(
'cpu')
self.gp_params = gp_params
self.obs_params = obs_params
self.planner_params = planner_params
self.optim_params = optim_params
self.env_params = env_params
self.robot_model = robot_model
self.learn_params = learn_params
self.batch_size = batch_size
self.dof = planner_params['dof']
self.state_dim = planner_params['state_dim']
self.total_time_sec = planner_params['total_time_sec']
self.total_time_step = planner_params[
'total_time_step'] #Number of states in the trajectory
self.num_traj_states = self.total_time_step + 1 #+1 for goal at the end
self.dt = self.total_time_sec * 1.0 / self.total_time_step * 1.0
self.non_holonomic = planner_params[
'non_holonomic'] if 'non_holonomic' in planner_params else False
if self.non_holonomic: self.num_dynamics_factors = self.num_traj_states
self.use_vel_limits = planner_params[
'use_vel_limits'] if 'use_vel_limits' in planner_params else False
if self.use_vel_limits:
self.num_vel_factors = self.num_traj_states
#Dimensions of the linear system
self.num_gp_factors = self.num_traj_states - 1
self.num_prior_factors = 2 #Prior on start and goal
self.num_obs_factors = self.num_traj_states
self.nlinks = self.robot_model.nlinks
self.M = self.state_dim * (self.num_gp_factors + self.num_prior_factors
) + self.num_obs_factors * self.nlinks
if self.non_holonomic: self.M = self.M + self.num_dynamics_factors
if self.use_vel_limits:
self.M = self.M + self.dof * self.num_vel_factors
self.N = self.state_dim * self.num_traj_states
if self.state_dim == 4:
self.env = Env2D(self.env_params, self.use_cuda)
#Create masks for forming A,b,K
self.create_factor_masks()
#Create factor objects
self.gp_prior = GPFactor(self.dof,
self.dt,
self.num_gp_factors,
batch_size=self.batch_size,
use_cuda=self.use_cuda)
self.start_prior = PriorFactor(self.state_dim,
self.gp_params['K_s'],
batch_size=self.batch_size,
use_cuda=self.use_cuda)
self.goal_prior = PriorFactor(self.state_dim,
self.gp_params['K_g'],
batch_size=self.batch_size,
use_cuda=self.use_cuda)
self.obs_factor = ObstacleFactor(self.state_dim, self.num_obs_factors,
self.obs_params['epsilon_dist'],
self.env_params, self.robot_model,
self.batch_size, self.use_cuda)
if self.non_holonomic:
self.dyn_factor = NonHolonomicFactor(self.dof,
self.gp_params['K_d'],
self.num_dynamics_factors,
self.batch_size,
self.use_cuda)
if self.use_vel_limits:
self.vel_factor = VelocityLimitFactor(self.state_dim,
self.num_vel_factors,
self.gp_params['K_v'],
self.batch_size,
self.use_cuda)
vx = torch.tensor(gp_params['v_x'], device=self.device)
vy = torch.tensor(gp_params['v_y'], device=self.device)
vx_traj = vx.unsqueeze(0).expand(self.num_vel_factors, 1)
vy_traj = vy.unsqueeze(0).expand(self.num_vel_factors, 1)
self.vel_factor.set_v_traj(vx_traj, vy_traj)
K_s_batch = mat_utils.isotropic_matrix(
1.0 / torch.pow(self.gp_params['K_s'], 2.0), self.state_dim,
self.device).unsqueeze(0).repeat(self.batch_size, 1, 1)
K_g_batch = mat_utils.isotropic_matrix(
1.0 / torch.pow(self.gp_params['K_g'], 2.0), self.state_dim,
self.device).unsqueeze(0).repeat(self.batch_size, 1, 1)
self.start_prior.set_inv_cov(K_s_batch)
self.goal_prior.set_inv_cov(K_g_batch)
#External cost function used to measure trajectory quality
qc_inv = self.gp_params['Q_c_inv']
sigma = self.obs_params['cost_sigma']
self.qc_inv_traj_fix = torch.zeros(self.batch_size,
self.num_gp_factors,
self.dof,
self.dof,
device=self.device)
self.qc_inv_traj_fix = self.qc_inv_traj_fix + qc_inv
inv_cov = mat_utils.isotropic_matrix(1.0 / torch.pow(sigma, 2.0),
self.robot_model.nlinks,
self.device)
self.obscov_inv_traj_fix = torch.zeros(self.batch_size,
self.num_traj_states,
self.robot_model.nlinks,
self.robot_model.nlinks,
device=self.device)
self.obscov_inv_traj_fix = self.obscov_inv_traj_fix + inv_cov
self.gp_prior_fix = GPFactor(self.dof,
self.dt,
self.num_gp_factors,
batch_size=self.batch_size,
use_cuda=self.use_cuda)
self.obs_factor_fix = ObstacleFactor(self.state_dim,
self.num_obs_factors,
self.obs_params['epsilon_dist'],
self.env_params, self.robot_model,
self.batch_size, self.use_cuda)
self.gp_prior_fix.set_Q_c_inv(self.qc_inv_traj_fix)
self.obs_factor_fix.set_inv_cov(self.obscov_inv_traj_fix)
if self.learn_params is not None:
self.dynamics_mode = self.learn_params['dgpmp2']['dynamics_mode']
def __init__(self, gp_params, obs_params, planner_params, optim_params, env_params, robot_model, learn_params=None, batch_size=1, use_cuda=False):
super(DiffGPMP2Planner, self).__init__()
self.use_cuda = torch.cuda.is_available() if use_cuda else False
self.device = torch.device('cuda') if self.use_cuda else torch.device('cpu')
self.dof = planner_params['dof']
self.state_dim = planner_params['state_dim']
self.total_time_sec = planner_params['total_time_sec']
self.total_time_step = planner_params['total_time_step'] #Number of states in the trajectory
self.num_traj_states = self.total_time_step + 1 #+1 for goal at the end
self.num_gp_factors = self.num_traj_states - 1
self.num_obs_factors = self.num_traj_states
self.optim_params = optim_params
self.gp_params = gp_params
self.obs_params = obs_params
self.robot_model = robot_model
self.env_params = env_params
self.learn_params = learn_params
self.model_type = 'feed_forward'
self.non_holonomic = planner_params['non_holonomic'] if 'non_holonomic' in planner_params else False
self.use_vel_limits = planner_params['use_vel_limits'] if 'use_vel_limits' in planner_params else False
if self.non_holonomic: self.num_dynamics_factors = self.num_traj_states
if self.use_vel_limits:
self.num_vel_factors = self.num_traj_states
self.batch_size=batch_size
if learn_params is None:
qc_inv = self.gp_params['Q_c_inv']
sigma = self.obs_params['cost_sigma']
eps = self.obs_params['epsilon_dist']
inv_cov = mat_utils.isotropic_matrix(1.0/torch.pow(sigma,2.0), self.robot_model.nlinks, self.device)
self.qc_inv_traj = torch.zeros(self.num_gp_factors, self.dof, self.dof, device=self.device)
self.qc_inv_traj = self.qc_inv_traj + qc_inv
self.obscov_inv_traj = torch.zeros(self.num_traj_states, self.robot_model.nlinks, 1, device=self.device)
self.obscov_inv_traj = self.obscov_inv_traj + inv_cov
self.eps_traj = torch.zeros(self.num_traj_states, self.robot_model.nlinks, 1, device=self.device)
self.eps_traj = self.eps_traj + eps
self.learn_module_conv = None
self.learn_module_fcn = None
else:
self.model_type = self.learn_params['model']['type'] if 'type' in self.learn_params['model'] else False
self.learn_eps = self.learn_params['dgpmp2']['learn_eps'] if 'learn_eps' in self.learn_params['dgpmp2'] else False
self.sdf_predict = self.learn_params['dgpmp2']['sdf_predict']
self.use_dtheta = self.learn_params['dgpmp2']['dtheta_predict'] if 'dtheta_predict' in self.learn_params['dgpmp2'] else False
self.res = (self.env_params['x_lims'][1] - self.env_params['x_lims'][0])/(self.learn_params['data']['im_size']*1.0)
learn_params['num_traj_states'] = self.num_traj_states
learn_params['state_dim'] = self.state_dim
if self.use_dtheta: learn_params['num_traj_states'] = 2 * self.num_traj_states
self.dynamics_mode = learn_params['dgpmp2']['dynamics_mode']
if self.dynamics_mode == 'fix_dynamics':
learn_params['out_dim'] = self.num_obs_factors*self.robot_model.nlinks
qc_inv = self.gp_params['Q_c_inv']
self.qc_inv_traj = torch.zeros(self.num_gp_factors, self.dof, self.dof, device=self.device)
self.qc_inv_traj = self.qc_inv_traj + qc_inv
elif self.dynamics_mode == 'diag_identity':
learn_params['out_dim'] = self.num_gp_factors + self.num_obs_factors*self.robot_model.nlinks
elif self.dynamics_mode == 'diag':
learn_params['out_dim'] = self.num_gp_factors* self.dof + self.num_obs_factors*self.robot_model.nlinks
elif self.dynamics_mode == 'qc_full':
learn_params['out_dim'] = self.num_gp_factors* self.dof + self.num_obs_factors*self.robot_model.nlinks
elif self.dynamics_mode == 'q_full':
learn_params['out_dim'] = self.num_gp_factors* self.state_dim + self.num_obs_factors*self.robot_model.nlinks
#if not learning epsilon, set it to user defined
if self.learn_eps:
learn_params['out_dim'] = learn_params['out_dim'] + self.num_obs_factors*self.robot_model.nlinks
else:
self.eps = self.obs_params['epsilon_dist']
self.eps_traj = torch.zeros(self.num_traj_states, self.robot_model.nlinks, 1, device=self.device)
self.eps_traj = self.eps_traj + self.eps
self.fixed_conv = learn_params['dgpmp2']['fixed_conv'] if 'fixed_conv' in learn_params['dgpmp2'] else False
# self.learn_module_conv = None
self.learn_module_conv = LearnModuleConv(learn_params, env_params, robot_model, use_cuda=self.use_cuda)
self.learn_module_fcn = LearnModuleFCN(learn_params, env_params, obs_params, robot_model, use_cuda=self.use_cuda)
self.plan_layer = PlanLayer(gp_params, obs_params, planner_params, optim_params, env_params, robot_model,learn_params,self.batch_size, self.use_cuda)