def main(self):
        rospy.init_node('servo_demo', disable_signals=True)

        self.info('Started.')
        contracts.disable_all()

        boot_root = rospy.get_param('~boot_root')
        boot_root = expand_environment(boot_root)

        config_dir = rospy.get_param('~config_dir')
        id_robot_learned = rospy.get_param('~id_robot_learn')

        self.info('loading %r' % config_dir)
        GlobalConfig.global_load_dir(config_dir)

        id_agent = rospy.get_param('~id_agent')
        self.id_robot = rospy.get_param('~id_robot')
        self.sleep = rospy.get_param('~sleep', 0.005)
        self.info('sleep: %s' % self.sleep)
        self.error_threshold = float(rospy.get_param('~error_threshold'))

        raise_if_no_state = rospy.get_param('~raise_if_no_state', True)

        data_central = DataCentral(boot_root)

        ag_st = load_agent_state(data_central,
                                 id_agent,
                                 id_robot_learned,
                                 reset_state=False,
                                 raise_if_no_state=raise_if_no_state)
        self.agent, state = ag_st

        self.info('Loaded state: %s' % state)

        self.servo_agent = self.agent.get_servo()

        bo_config = get_boot_config()
        self.robot = bo_config.robots.instance(self.id_robot)
        self.boot_spec = self.robot.get_spec()

        self.publish_info_init()

        self.y = None
        self.y_goal = None
        self.started_now = False
        self.stopped_now = False
        self.e0 = 1
        self.e = 1
        self.last_boot_data = None
        self.state = STATE_WAIT

        self.info('Defining services')
        rospy.Service('set_goal', Empty, self.srv_set_goal)
        rospy.Service('start_servo', Empty, self.srv_start_servo)
        rospy.Service('stop_servo', Empty, self.srv_stop_servo)

        self.info('Finished initialization')
        self.count = 0
        self.go()
    def main(self):
        rospy.init_node('servo_demo', disable_signals=True)
        
        self.info('Started.')
        contracts.disable_all()

        boot_root = rospy.get_param('~boot_root')
        boot_root = expand_environment(boot_root)
        
        config_dir = rospy.get_param('~config_dir')
        id_robot_learned = rospy.get_param('~id_robot_learn')
        
        self.info('loading %r' % config_dir)
        GlobalConfig.global_load_dir(config_dir)
        
        id_agent = rospy.get_param('~id_agent')
        self.id_robot = rospy.get_param('~id_robot')
        self.sleep = rospy.get_param('~sleep', 0.005)
        self.info('sleep: %s' % self.sleep)
        self.error_threshold = float(rospy.get_param('~error_threshold'))
        
        raise_if_no_state = rospy.get_param('~raise_if_no_state', True)
        
        data_central = DataCentral(boot_root)
        
        ag_st = load_agent_state(data_central, id_agent, id_robot_learned,
                                 reset_state=False,
                                 raise_if_no_state=raise_if_no_state)
        self.agent, state = ag_st
        
        self.info('Loaded state: %s' % state)
        
        self.servo_agent = self.agent.get_servo()
        
        bo_config = get_boot_config()
        self.robot = bo_config.robots.instance(self.id_robot)
        self.boot_spec = self.robot.get_spec()
            
        self.publish_info_init()     
        
        self.y = None
        self.y_goal = None
        self.started_now = False
        self.stopped_now = False
        self.e0 = 1
        self.e = 1         
        self.last_boot_data = None
        self.state = STATE_WAIT

        self.info('Defining services')
        rospy.Service('set_goal', Empty, self.srv_set_goal)
        rospy.Service('start_servo', Empty, self.srv_start_servo)
        rospy.Service('stop_servo', Empty, self.srv_stop_servo)
                
        self.info('Finished initialization') 
        self.count = 0
        self.go()
Beispiel #3
0
def report_prediction2(data_central, id_agent, id_robot, exp=myexp): 
    set_boot_config(data_central.get_bo_config())
    agent, state = load_agent_state(data_central, id_agent, id_robot,
                                    reset_state=False, raise_if_no_state=True)
    model = agent.estimator.get_model()
    warnings.warn('does not work anymore')
    slt = BDSEServoLongTerm()
    slt.set_model(model)
    return  slt.report()
Beispiel #4
0
def report_prediction(data_central, id_agent, id_robot, exp=myexp): 
    set_boot_config(data_central.get_bo_config())
    agent, state = load_agent_state(data_central, id_agent, id_robot,
                                    reset_state=False, raise_if_no_state=True)
    
    r = Report('report-pred')
    f = r.figure()
    model = agent.estimator.get_model()
#     R = agent.estimator.y_stats.get_correlation()
    M = model.M
    N, _, K = M.shape 

#     W = np.eye(N)
    W = np.zeros((N, N))
    for i, j in itertools.product(range(N), range(N)):
        if np.abs(i - j) == 1:
            W[i, j] = 1
#     W = R * R * R * R * R
    f.data('W', W).display('posneg').add_to(f, caption='W')

    times = [0.25, 0.5, 1, 2, 4, 8, 16]
    for k in  range(K):
        A = M[:, :, k]
#         AW = A * W
#         AW = (AW - AW.T) / 2
        report_exp(r, 'k=%d' % k, A, times, exp=exp)
        report_exp(r, 'k=%dneg' % k, -A, times, exp=exp)
        

    for k1, k2 in itertools.product(range(K), range(K)):
        A = M[:, :, k1] + M[:, :, k2] 
        report_exp(r, 'k1=%d,k2=%d' % (k1, k2), A, times, exp=exp)
        

        
#     D1 = np.zeros((N, N))
#     for i, j in itertools.product(range(N), range(N)):
#         if i == j + 2:
#             D1[i, j] = 1
#         if i == j + 1:
#             D1[i, j] = 2
#         if i == j - 1:
#             D1[i, j] = -2
#         if i == j - 2:
#             D1[i, j] = -1
# 
#     report_exp(r, 'D1', D1, times, exp=exp)
    
    return r