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()