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);
}
