def __init__(self,
connected_init=False,
family=socket.AF_INET,
socket_type=socket.SOCK_STREAM,
*args,
**kwargs):
if not connected_init:
self.logger = utils.init_logger('RobotServer')
else:
self.logger = utils.get_logger('RobotServer')
super().__init__(family=family, type=socket_type, *args, **kwargs)
self.init_robot = parameters.INIT_ROBOT
# a robot controller and sweep controller should only be created if a client connects to the server
if connected_init:
# a robot controller should only be created if the user wishes to init a robot
if self.init_robot:
self.rob = robcon.RobotController()
self.sweep_controller = sweep.SweepMeasurement(
nfft=parameters.SWEEP_LENGTH * parameters.SWEEP_FS,
fs=parameters.SWEEP_FS)
self.logger.info('Successfully initialized a RobotServer.')
def createRobot(robotConfig, resultQueue):
r = robot_controller.RobotController(robotConfig)
resultQueue.put(r)
def __init__(self):
parser = argparse.ArgumentParser(description='Run benchmarks')
parser.add_argument('benchmark', type=str, help='the name of the folder with agent setting for the benchmark')
parser.add_argument('--goalimage', default='False', help='whether to collect goalimages')
parser.add_argument('--save_subdir', default='False', type=str, help='')
parser.add_argument('--canon', default=-1, type=int, help='whether to store canonical example')
args = parser.parse_args()
self.base_dir = '/'.join(str.split(base_filepath, '/')[:-2])
cem_exp_dir = self.base_dir + '/experiments/cem_exp/benchmarks_sawyer'
benchmark_name = args.benchmark
bench_dir = cem_exp_dir + '/' + benchmark_name
if not os.path.exists(bench_dir):
raise ValueError('benchmark directory does not exist')
bench_conf = imp.load_source('mod_hyper', bench_dir + '/mod_hyper.py')
self.policyparams = bench_conf.policy
self.agentparams = bench_conf.agent
self.benchname = benchmark_name
self.args = args
if 'ndesig' in self.policyparams:
self.ndesig = self.policyparams['ndesig']
else: self.ndesig = 1
if args.canon != -1:
self.save_canon =True
self.canon_dir = '/home/guser/catkin_ws/src/lsdc/pushing_data/canonical_singleobject'
self.canon_ind = args.canon
pdb.set_trace()
else:
self.save_canon = False
self.canon_dir = ''
self.canon_ind = None
self.num_traj = 50
self.action_sequence_length = self.agentparams['T'] # number of snapshots that are taken
self.use_robot = True
self.robot_move = True
self.save_subdir = ""
self.use_aux = False
if self.use_robot:
self.ctrl = robot_controller.RobotController()
self.get_action_func = rospy.ServiceProxy('get_action', get_action)
self.init_traj_visual_func = rospy.ServiceProxy('init_traj_visualmpc', init_traj_visualmpc)
if self.use_robot:
self.imp_ctrl_publisher = rospy.Publisher('desired_joint_pos', JointState, queue_size=1)
self.imp_ctrl_release_spring_pub = rospy.Publisher('release_spring', Float32, queue_size=10)
self.imp_ctrl_active = rospy.Publisher('imp_ctrl_active', Int64, queue_size=10)
self.name_of_service = "ExternalTools/right/PositionKinematicsNode/FKService"
self.fksvc = rospy.ServiceProxy(self.name_of_service, SolvePositionFK)
self.use_imp_ctrl = True
self.interpolate = True
self.save_active = True
self.bridge = CvBridge()
self.action_interval = 1 #Hz
self.traj_duration = self.action_sequence_length*self.action_interval
self.action_rate = rospy.Rate(self.action_interval)
self.control_rate = rospy.Rate(1000)
rospy.sleep(.2)
# drive to neutral position:
self.imp_ctrl_active.publish(0)
self.ctrl.set_neutral()
self.set_neutral_with_impedance()
self.imp_ctrl_active.publish(1)
rospy.sleep(.2)
self.goal_pos_main = np.zeros([2,2]) # the first index is for the ndesig and the second is r,c
self.desig_pos_main = np.zeros([2, 2])
if args.goalimage == "True":
self.use_goalimage = True
else: self.use_goalimage = False
self.run_visual_mpc()
