action_result assert_cc_action(hypothesis_idx hidx) { if (!get_config().m_cc) return action_result::failed(); congruence_closure & cc = get_cc(); if (has_expr_metavar(curr_state().get_hypothesis_decl(hidx).get_type())) return action_result::failed(); cc.add(hidx); // cc.display(); if (cc.is_inconsistent()) { try { app_builder & b = get_app_builder(); expr false_proof = *cc.get_inconsistency_proof(); trace_action("contradiction by congruence closure"); return action_result(b.mk_false_rec(curr_state().get_target(), false_proof)); } catch (app_builder_exception &) { return action_result::failed(); } } else { expr const & target = curr_state().get_target(); name R; expr lhs, rhs; if (is_relation_app(target, R, lhs, rhs) && cc.is_eqv(R, lhs, rhs)) { expr proof = *cc.get_eqv_proof(R, lhs, rhs); trace_action("equivalence by congruence closure"); return action_result(proof); } else if (is_prop(target) && !is_false(target) && cc.proved(target)) { expr proof = *cc.get_proof(target); trace_action("equivalent to true by congruence closure"); return action_result(proof); } else { return action_result::new_branch(); } } }
bool encoder::process() { guard g(m_lock); if (curr_state() == STATE_FULL) { if (is_timed_out(FLAGS_encoder_timeout*5)) { inc("blocked timeouts"); LOG(ERROR) << "Encoder " << m_coder << ": Timed out while blocked"; enc_notify(); return true; } return false; } /* check if decoder is ready to be reused */ if (curr_state() == STATE_DONE) return true; /* check if decoder is timed out */ if (is_timed_out()) { LOG(ERROR) << "Encoder " << m_coder << ": Timed out (rank " << m_plain_pkt_count << ", state " << static_cast<int>(curr_state()) << ")"; dispatch_event(EVENT_TIMEOUT); inc("timeouts"); if (m_plain_pkt_count == this->symbols()) enc_notify(); } return false; }
static void route_event(StateMachine * sm, gint event) { StateFunc curr_state; gpointer user_data; if (sm->stack_ptr >= 0) curr_state = sm_current(sm); else curr_state = NULL; user_data = sm->user_data; if (user_data == NULL) user_data = sm; /* send death notification even when dead */ if (event == SM_FREE) { /* send death notifications only to global handler */ if (sm->global !=NULL) sm->global (user_data, event); return; } if (sm->is_dead) return; switch (event) { case SM_ENTER: if (curr_state != NULL) curr_state(user_data, event); break; case SM_INIT: if (curr_state != NULL) curr_state(user_data, event); if (!sm->is_dead && sm->global !=NULL) sm->global (user_data, event); break; case SM_RECV: sm_cancel_prefix(sm); if (curr_state != NULL && curr_state(user_data, event)) break; sm_cancel_prefix(sm); if (!sm->is_dead && sm->global !=NULL && sm->global (user_data, event)) break; sm_cancel_prefix(sm); if (!sm->is_dead && sm->unhandled != NULL) sm->unhandled(user_data, event); break; case SM_NET_CLOSE: sm_close(sm); default: if (curr_state != NULL) curr_state(user_data, event); if (!sm->is_dead && sm->global !=NULL) sm->global (user_data, event); break; } }
int goto_pause(struct state *s) { if (curr_state() == &st_pause) return 1; st_continue = curr_state(); st_quit = s; paused = 1; return goto_state(&st_pause); }
int goto_pause(struct state *s, int e) { if (curr_state() == &st_pause) return 1; if (e && !config_tst_d(CONFIG_KEY_PAUSE, SDLK_ESCAPE)) return goto_state(s); st_continue = curr_state(); st_quit = s; paused = 1; return goto_state(&st_pause); }
virtual action_result resolve(expr const & pr) const override { try { expr it = pr; bool skip = true; for (unsigned i = 0; i < m_num_new_eqs; i++) { if (!is_lambda(it)) { break; skip = false; } it = binding_body(it); } if (skip && closed(it)) { // new eq hypotheses were not used return action_result::solved(it); } state & s = curr_state(); app_builder & b = get_app_builder(); hypothesis const & h = s.get_hypothesis_decl(href_index(m_eq_href)); expr type = h.get_type(); expr lhs, rhs; lean_verify(is_eq(type, lhs, rhs)); name nc_name(m_I_name, "no_confusion"); expr new_pr = mk_app(b.mk_app(nc_name, {m_target, lhs, rhs, m_eq_href}), pr); return action_result::solved(new_pr); } catch (app_builder_exception &) { return action_result::failed(); } }
void OpenBagMotionPlanner::WaitForExecution() { ROS_INFO_STREAM("START WAITING"); robot_state::RobotState prev_state(*groupPtr->getCurrentState()); robot_state::RobotState curr_state(*groupPtr->getCurrentState()); bool moving = true; ros::Duration(2.0).sleep(); while (moving) { ros::Duration(1.0).sleep(); curr_state = *groupPtr->getCurrentState(); std::vector<double> diffs; diffs.push_back(*(curr_state.getJointPositions(JOINT1NAME)) - *(prev_state.getJointPositions(JOINT1NAME))); diffs.push_back(*(curr_state.getJointPositions(JOINT2NAME)) - *(prev_state.getJointPositions(JOINT2NAME))); diffs.push_back(*(curr_state.getJointPositions(JOINT3NAME)) - *(prev_state.getJointPositions(JOINT3NAME))); diffs.push_back(*(curr_state.getJointPositions(JOINT4NAME)) - *(prev_state.getJointPositions(JOINT4NAME))); diffs.push_back(*(curr_state.getJointPositions(JOINT5NAME)) - *(prev_state.getJointPositions(JOINT5NAME))); diffs.push_back(*(curr_state.getJointPositions(JOINT6NAME)) - *(prev_state.getJointPositions(JOINT6NAME))); for (std::vector<double>::iterator diff_it = diffs.begin(); diff_it != diffs.end(); ++diff_it) { if (fabs(*diff_it) > 0.0005) { moving = true; break; } else { moving = false; } } prev_state = curr_state; } ROS_INFO_STREAM("STOP WAITING"); }
action_result intros_action(unsigned max) { if (max == 0) return action_result::new_branch(); state & s = curr_state(); expr target = whnf(s.get_target()); if (!is_pi(target)) return action_result::failed(); auto pcell = new intros_proof_step_cell(); s.push_proof_step(pcell); buffer<expr> new_hs; for (unsigned i = 0; i < max; i++) { if (!is_pi(target)) break; expr href; expr htype = head_beta_reduce(binding_domain(target)); if (is_default_var_name(binding_name(target)) && closed(binding_body(target))) { href = s.mk_hypothesis(htype); } else { href = s.mk_hypothesis(binding_name(target), htype); } new_hs.push_back(href); target = whnf(instantiate(binding_body(target), href)); } pcell->m_new_hs = to_list(new_hs); s.set_target(target); trace_action("intros"); return action_result::new_branch(); }
void recoder::send_rec_redundant() { VLOG(LOG_PKT) << "Recoder " << m_coder << ": Sending redundant packets (state: " << static_cast<int>(curr_state()) << ")"; guard g(m_lock); send_rec_packet(); }
action_result no_confusion_action(hypothesis_idx hidx) { try { state & s = curr_state(); app_builder & b = get_app_builder(); hypothesis const & h = s.get_hypothesis_decl(hidx); expr type = h.get_type(); expr lhs, rhs; if (!is_eq(type, lhs, rhs)) return action_result::failed(); lhs = whnf(lhs); rhs = whnf(rhs); optional<name> c1 = is_constructor_app(env(), lhs); optional<name> c2 = is_constructor_app(env(), rhs); if (!c1 || !c2) return action_result::failed(); expr A = whnf(infer_type(lhs)); expr I = get_app_fn(A); if (!is_constant(I) || !inductive::is_inductive_decl(env(), const_name(I))) return action_result::failed(); name nct_name(const_name(I), "no_confusion_type"); if (!env().find(nct_name)) return action_result::failed(); expr target = s.get_target(); expr nct = whnf(b.mk_app(nct_name, target, lhs, rhs)); if (c1 == c2) { if (!is_pi(nct)) return action_result::failed(); if (s.has_target_forward_deps(hidx)) { // TODO(Leo): we currently do not handle this case. // To avoid non-termination we remove the given hypothesis, if there // forward dependencies, we would also have to remove them. // Remark: this is a low priority refinement since it will not happen // very often in practice. return action_result::failed(); } unsigned num_params = *inductive::get_num_params(env(), const_name(I)); unsigned cnstr_arity = get_arity(env().get(*c1).get_type()); lean_assert(cnstr_arity >= num_params); unsigned num_new_eqs = cnstr_arity - num_params; s.push_proof_step(new no_confusion_proof_step_cell(const_name(I), target, h.get_self(), num_new_eqs)); s.set_target(binding_domain(nct)); s.del_hypothesis(hidx); trace_action("no_confusion"); return action_result::new_branch(); } else { name nc_name(const_name(I), "no_confusion"); expr pr = b.mk_app(nc_name, {target, lhs, rhs, h.get_self()}); trace_action("no_confusion"); return action_result::solved(pr); } } catch (app_builder_exception &) { return action_result::failed(); } }
strategy iterative_deepening(strategy const & S, unsigned init, unsigned inc, unsigned max) { return [=]() { // NOLINT state s = curr_state(); unsigned ncs = get_num_choice_points(); unsigned d = init; while (true) { flet<unsigned> set_depth(get_config().m_max_depth, d); if (auto r = S()) return r; d += inc; if (d > max) { if (get_config().m_show_failure) display_curr_state(); return none_expr(); } curr_state() = s; shrink_choice_points(ncs); }; }; }
action_result grinder_intro_action() { grinder_branch_extension & ext = get_ext(); state & s = curr_state(); expr const & target = s.get_target(); expr const & f = get_app_fn(target); if (!is_constant(f)) return action_result::failed(); list<gexpr> const * lemmas = ext.m_intro_lemmas.find(head_index(f)); if (!lemmas) return action_result::failed(); return backward_cut_action(*lemmas); }
action_result grinder_elim_action(hypothesis_idx hidx) { grinder_branch_extension & ext = get_ext(); state & s = curr_state(); hypothesis const & h = s.get_hypothesis_decl(hidx); expr const & f = get_app_fn(h.get_type()); if (!is_constant(f)) return action_result::failed(); auto R = ext.m_elim_lemmas.find(const_name(f)); if (!R) return action_result::failed(); return recursor_action(hidx, *R); }
strategy grind_and_then(strategy const & S) { return [=]() { // NOLINT curr_state().deactivate_all(); return grinder_strategy_fn([](hypothesis_idx) { return action_result::failed(); }, // NOLINT [](hypothesis_idx) { return action_result::failed(); }, // NOLINT [=]() { // NOLINT if (optional<expr> pf = S()) { return action_result::solved(*pf); } return action_result::failed(); })(); }; }
bool decoder::process() { if (curr_state() == STATE_DONE) return true; if (is_timed_out() && !this->is_complete() && !this->is_partial_complete()) { LOG(ERROR) << "Decoder " << m_coder << ": Timed out (rank " << this->rank() << ")"; inc("incomplete timeouts"); dispatch_event(EVENT_TIMEOUT); return false; } if (is_timed_out()) { dispatch_event(EVENT_TIMEOUT); return false; } if (curr_state() == STATE_WAIT && packet_timed_out()) { if (this->is_partial_complete()) return false; double req_budget = source_budget(1, 254, 254, m_e3); VLOG(LOG_GEN) << "Decoder " << m_coder << ": Request more data (rank " << this->rank() << ", seq " << m_req_seq << ")"; for (; req_budget >= 0; req_budget--) send_request(m_req_seq); m_req_seq++; update_packet_timestamp(); return false; } return false; }
void recoder::add_enc_packet(const uint8_t *data, const uint16_t len) { size_t tmp_rank; guard g(m_lock); /* don't add packets when we have enough, and try to stop encoder * from sending more packets */ if (this->is_complete()) { send_ack_packet(); return; } if (curr_state() == STATE_DONE) return; CHECK_EQ(len, this->payload_size()) << "Recoder " << m_coder << ": Encoded data is too short:" << len << " < " << this->payload_size(); /* keep track of changes in rank */ tmp_rank = this->rank(); this->decode(const_cast<uint8_t *>(data)); /* check if rank improved */ if (this->rank() == tmp_rank) inc("non-innovative recoded packets"); update_timestamp(); if (this->last_symbol_is_systematic()) { inc("systematic packets added"); send_systematic_packet(data, len); m_budget--; } else { inc("encoded packets added"); } /* signal state machine if generation is complete */ if (this->is_complete()) { send_ack_packet(); dispatch_event(EVENT_COMPLETE); } else { dispatch_event(EVENT_RX); } VLOG(LOG_PKT) << "Recoder " << m_coder << ": Added encoded packet"; }
void encoder::add_ack_packet() { guard g(m_lock); if (curr_state() == STATE_DONE) return; if (m_plain_pkt_count == this->symbols()) enc_notify(); dispatch_event(EVENT_ACKED); inc("ack packets added"); VLOG(LOG_CTRL) << "Encoder " << m_coder << ": Acked after " << m_enc_pkt_count << " packets"; }
bool recoder::process() { /* check if done coding */ if (curr_state() == STATE_DONE) return true; /* check if timed out */ if (is_timed_out()) { VLOG(LOG_GEN) << "Recoder " << m_coder << ": Timed out"; dispatch_event(EVENT_TIMEOUT); return false; } return false; }
// Requirements: // No mallocs // Simple system calls so that it works on linux or windows. // Assumptions: // Lines with nested c-style comments are not given. // Tested with buffer that 256 bytes long. // int remove_comments(char *str) { int i,iLen; // init our trim positions trim_start = -1; trim_end = -1; if (NULL == str) { // null string. return EXIT_FAILURE; } iLen = strlen(str); if ('/' == str[0]) { curr_state = s0; // printf("s0 %d %c\n",0, str[0]); } else { curr_state = s4; // printf("s4 %d %c\n",0, str[0]); } // Look for C comments for (i=1;i<iLen;i++) { curr_state(str[i],i); } // printf("Trim start = %d\n",trim_start); // printf("Trim end = %d\n",trim_end); if (-1 != trim_start && -1 != trim_end) { if (254 == trim_end ) { // printf("special c++ thing\n"); str[trim_start] = 0; } else { memmove(&str[trim_start],&str[trim_end+1],trim_end-trim_start); } } // Look for C++ comments return EXIT_SUCCESS; }
action_result by_contradiction_action() { state & s = curr_state(); expr target = whnf(s.get_target()); if (!is_prop(target)) return action_result::failed(); if (blast::is_false(target)) return action_result::failed(); expr not_target; if (is_not(target, not_target)) { s.set_target(mk_arrow(not_target, mk_constant(get_false_name()))); return intros_action(1); } blast_tmp_type_context tmp_tctx; optional<expr> target_decidable = tmp_tctx->mk_class_instance(mk_app(mk_constant(get_decidable_name()), target)); if (!target_decidable) return action_result::failed(); expr href = s.mk_hypothesis(get_app_builder().mk_not(target)); auto pcell = new by_contradiction_proof_step_cell(href); s.push_proof_step(pcell); s.set_target(mk_constant(get_false_name())); trace_action("by_contradiction"); return action_result::new_branch(); }
void encoder::add_plain_packet(const uint8_t *data, const uint16_t len) { uint8_t *buf; size_t size = this->symbol_size(); CHECK_LE(len, size - LEN_SIZE) << "Encoder " << m_coder << ": Plain packet is too long: " << len << " > " << size - LEN_SIZE; guard g(m_lock); /* make sure encoder is in a state to accept plain packets */ if (curr_state() != STATE_WAIT) return; buf = get_symbol_buffer(m_plain_pkt_count); *reinterpret_cast<uint16_t *>(buf) = len; memcpy(buf + LEN_SIZE, data, len); /* Copy data into encoder storage */ sak::mutable_storage symbol(buf, this->symbol_size()); this->set_symbol(m_plain_pkt_count++, symbol); update_timestamp(); inc("plain packets added"); VLOG(LOG_PKT) << "Encoder " << m_coder << ": Added plain packet"; if (is_full()) { inc("generations"); dispatch_event(EVENT_FULL); } else if (this->rank() > FLAGS_encoder_threshold*this->symbols() && semaphore_count() > 0) { m_budget += recoder_credit(m_e1, m_e2, m_e3); send_encoded_credit(); } }
void System::get_kalman_path( vector<State>& obs, vector<double>& obs_times, list<State>& kalman_path) { #if 0 kalman_path.clear(); kalman_path.push_back(init_state); Matrix3d Q; Q << init_var[0], 0, 0, 0, init_var[1], 0, 0, 0, init_var[2]; Vector3d curr_state; curr_state(0) = init_state.x[0]; curr_state(1) = init_state.x[1]; curr_state(2) = init_state.x[2]; MatrixXd Rk(3,3); Rk << obs_noise[0], 0, 0, 0, obs_noise[1], 0, 0, 0, obs_noise[2]; Matrix3d Cd = Matrix3d::Identity(); double prev_time = 0; for(unsigned int i=0; i< obs.size(); i++) { State& next_obs = obs[i]; Vector3d noisy_obs; noisy_obs(0) = next_obs.x[0]; noisy_obs(1) = next_obs.x[1]; noisy_obs(2) = next_obs.x[2]; double delta_t = obs_times[i] - prev_time; //cout<<"delta_t: "<< delta_t << endl; State stmp1; stmp1.x[0] = curr_state(0); stmp1.x[1] = curr_state(1); stmp1.x[2] = curr_state(2); State next_state = integrate(stmp1, delta_t, true); State clean_obs = observation(next_state, true); Matrix3d Ad; Ad << 0, 0, -sin(stmp1.x[2]), 0, 0, cos(stmp1.x[2]), 0, 0, 0; Matrix3d Wk; Wk << process_noise[0]*delta_t, 0, 0, 0, process_noise[1]*delta_t, 0, 0, 0, process_noise[2]*delta_t; Matrix3d Q_new = Ad * Q * Ad.transpose() + Wk; Matrix3d Lk = Q_new*Cd.transpose()*(Cd*Q_new*Cd.transpose() + Rk).inverse(); Vector3d Sk; curr_state(0) = next_state.x[0]; curr_state(1) = next_state.x[1]; curr_state(2) = next_state.x[2]; Sk = noisy_obs - Cd*curr_state; Vector3d estimate = curr_state + Lk*Sk; Matrix3d covar = (Matrix3d::Identity() - Lk*Cd)*Q_new; Q = covar; curr_state = estimate; State stmp2; stmp2.x[0] = curr_state(0); stmp2.x[1] = curr_state(1); stmp2.x[2] = curr_state(2); kalman_path.push_back(stmp2); prev_time = obs_times[i]; } #endif }
static grinder_branch_extension & get_ext() { return static_cast<grinder_branch_extension&>(curr_state().get_extension(g_ext_id)); }
virtual action_result resolve(expr const & pf) const override { expr new_pf = mk_proof_lambda(curr_state(), m_href, pf); return action_result::solved(mk_proof_app(get_decidable_by_contradiction_name(), 1, &new_pf)); }
forward_branch_extension & get_forward_extension() { return static_cast<forward_branch_extension&>(curr_state().get_extension(g_ext_id)); }
virtual action_result resolve(expr const & pr) const override { expr new_pr = mk_proof_lambda(curr_state(), m_new_hs, pr); return action_result::solved(new_pr); }