def __init__(self, hyperparams, init_node=True, trace=True):
self.fixed_pose = Pose(Point(0.5, -0.1628, 0.5),
Quaternion(0.5, -0.5, -0.5, 0.5))
# self.fixed_pose = Pose(Point(0.35, -0.1628, 0.5), Quaternion(0.5, -0.5, -0.5, 0.5))
# with open('/home/gwthomas/.gazebo/models/fixed_piece/model-static.sdf', 'r') as f:
with open('/home/gwthomas/.gazebo/models/piece/model-static.sdf',
'r') as f:
self.fixed_piece_xml = f.read()
AgentCAD.__init__(self, hyperparams, init_node)
if self._hyperparams['use_AR_markers']:
self.ar = {
'held_piece': 0,
'fixed_piece': 1
} # Number of AR tag they have on them
# Create the functions with the proper offsets and whatever
self.ar_functions[self.ar['held_piece']] = self.create_AR_function( \
self.ar['held_piece'], 0.003, -0.02, -0.0265, 0, 0, -1.57)
#self.ar['held_piece'], 0.035, 0, -0.0465, 0, 0, -1.57)
self.ar_functions[self.ar['fixed_piece']] = self.create_AR_function( \
self.ar['fixed_piece'], 0, -0.025, -0.0325, 0, 0, 0)
self.hack = -1.0
self.try_mp = False
if trace:
pdb.set_trace() # for optional setup, not debugging
def __init__(self, hyperparams, init_node=True):
AgentCAD.__init__(self, hyperparams, init_node)
self.dists = [] # Get a list of the distances in the trajs
self.sample_dists = [] # For collecting the distances for each sample
self.attempts = 5
self.samples_taken = [-1] * 5 # How many samples have been taken
self.num_samples = 5
self.cur_frac = [1] * 5 # What fraction
self.full_ref_ee = [0] * 5 # Lol for the reference ee
self.full_ref_ja = [0] * 5 # For the full reference ja
self.prev_pose = None # For storing previous pose of gear lmao
self.cur_T = [0] * 5 # For storing the T of each of the conditions!
self.final_T = self.T # This is the original T WOWOWOWOWOWOWOWWOW
self.varying_T = False # If you want T to vary depending on a whole bunch of stuff
self.the_tolerance = 0.03 # If the difference is that large it's concerning
self.segments = 1.0 # How many segments to break the trajectory into
self.iter_per_seg = 3 # Let's train this many iterations per segment
self.iter_count = 0 # Count iterations
self.padding = 20 # How many timesteps to put as padding for each of the segments
self.chosen_parts = None #[145, 230] # SET THIS TO NONE IF YOU WANT TO USE FRACTIONS LMAO
self.use_saved_traj = False # If we already have one saved (pickled DisplayTrajectory)
self.pickled_traj_filename = 'pickled_robot_traj_cond0' # Where you saved the DisplayTrajectory
self.plan_filename = 'new_saved_robot_plan'
self.data_files_dir = hyperparams['data_files_dir']
self.saved_samples = [[] * 5] # Just for storing this real quick
self.unpickle_and_set(
hyperparams) # Check if we are resuming from some state
pdb.set_trace() # for optional setup, not debugging
def reset(self, condition):
try:
self.delete_model('fixed_piece')
except:
pass
AgentCAD.reset(self, condition)
self.spawn_model('fixed_piece', self.fixed_piece_xml, self.fixed_pose)
def __init__(self, hyperparams, init_node=True, trace=True):
#self.cur_T = [0] * 5 # For storing the T of each of the conditions!
#self.final_T = self.T # This is the original T WOWOWOWOWOWOWOWWOW
#self.varying_T = False # If you want T to vary depending on a whole bunch of stuff
#self.the_tolerance = 0.01 # If the difference is that large it's concerning
self.fixed_pose = Pose(Point(0.5, -0.1628, 0.5), Quaternion(0.5, -0.5, -0.5, 0.5))
self.diffPos, self.diffOri = None, None # Just set these
AgentCAD.__init__(self, hyperparams, init_node)
self.ar = {'held_piece': 4, 'fixed_piece': 24} # Number of AR tag they have on them
# Create the functions with the proper offsets and whatever
self.ar_functions[self.ar['held_piece']] = self.create_AR_function( \
self.ar['held_piece'], 0.003 - 0.01, -0.02, -0.026, 1.57 - 0.1, 0, -1.57) #0, 1.57, 0)
#self.ar['held_piece'], 0.035, 0, -0.0465, 0, 0, -1.57)
self.ar_functions[self.ar['fixed_piece']] = self.create_AR_function( \
#self.ar['fixed_piece'], -0.015, -0.025, -0.0325 - 0.015, 1.57, 0, 0)
self.ar['fixed_piece'], -0.015 + 0.0165 + 0.01, -0.025 + 0.005, -0.032 - 0.02, 0, 0, 0)
#self.wipe_plans() # Get rid of all the plans because we need to replan
if trace:
pdb.set_trace() # for optional setup, not debugging
def __init__(self, hyperparams, init_node=True):
AgentCAD.__init__(self, hyperparams, init_node)
self.trajs = [] # Get a list of trajectories and stuff
self.dists = [] # Get a list of the distances in the trajs
self.attempts = 50
self.use_saved_traj = False # If we already have one saved (pickled DisplayTrajectory)
self.pickled_traj_filename = '' # Where you saved the DisplayTrajectory
pdb.set_trace() # for optional setup, not debugging
def __init__(self, hyperparams, init_node=True, trace=True):
self.fixed_pose = Pose(Point(0.5, -0.1628, 0.5), Quaternion(0.5, -0.5, -0.5, 0.5))
with open(osp.join(hyperparams['exp_dir'], 'model-static.sdf'), 'r') as f:
self.fixed_piece_xml = f.read()
AgentCAD.__init__(self, hyperparams, init_node)
self.hack = -1.0
self.try_mp = False
self.wipe_plans() # Make sure the condition info does not get saved
self.setup()
self.grasp()
def __init__(self, hyperparams, init_node=True):
AgentCAD.__init__(self, hyperparams, init_node)
# Number of AR tag they have on them
self.ar = {'shaft2': 1, 'base_plate': 2}
# Create the functions with the proper offsets and whatever
self.ar_functions[self.ar['shaft2']] = self.create_AR_function( \
self.ar['shaft2'], -0.027, -0.01, -0.0067, -3.14, 0, 3.14)
self.ar_functions[self.ar['base_plate']] = self.create_AR_function( \
self.ar['base_plate'], -0.005, -0.025, 0.05, 0.0067, 0, 0)
self.plans_made = False # Plans made or not
self.stored_poses = {} # Stored poses for the objects in the scene
self.change_conds_to_goals() # Because we want to reverse plan
#self.wipe_plans() # Get rid of all the plans because we need to replan
pdb.set_trace() # Need to stop and setup
def setup_agent(cfg):
np.random.seed()
condition_info = cfg['agent']['condition_info']
targets = []
for i, info in enumerate(condition_info):
position = np.random.uniform(EE_LOW, EE_HIGH)
orientation = np.random.uniform(ANGLE_LOW, ANGLE_HIGH)
targets.append({'position': position, 'orientation': orientation})
info.initial = np.random.uniform(JA_LOW, JA_HIGH)
cfg['agent']['reset_conditions'][i][TRIAL_ARM]['data'] = info.initial
cond = int(re.findall('[0-9]{2}', info.path)[-1])
info.path = osp.join(DIR, 'info%02d.pkl' % cond)
info.data_path = osp.join(DIR, 'cond%02d.pkl' % cond)
cfg['agent']['targets'] = targets
return AgentCAD(cfg['agent'])
def reset(self, condition):
#try:
# self.delete_model('fixed_piece')
#except:
# pass
AgentCAD.reset(self, condition)