// sets p_hat to the across-skill state right before the trial = first_exposure
// used to reduce redundant computation during gibbs sampling
void MixtureWCRP::cache_p_hat(const size_t student, const size_t end_trial, boost::unordered_map<size_t, double> & p_hat) const {
// define some references for convenience:
const vector<bool> & recall_sequence = recall_sequences.at(student);
const vector<size_t> & item_sequence = item_sequences.at(student);
// initialize p_hat
for (boost::unordered_map<size_t, struct bkt_parameters>::const_iterator table_itr = parameters.begin(); table_itr != parameters.end(); table_itr++) p_hat[table_itr->first] = table_itr->second.psi;
for (size_t trial = 0; trial < end_trial; trial++) {
// define some constants for notational clarity
const bool did_recall = recall_sequence.at(trial);
const size_t table_id = seating_arrangement.at(item_sequence.at(trial));
const struct bkt_parameters & skill_params = parameters.at(table_id);
const double skill_pi1 = skill_params.pi1;
const double skill_pi0 = skill_pi1 * skill_params.prop0;
const double skill_mu = skill_params.mu;
const double cur_p_hat = p_hat.at(table_id);
const double one_minus_cur_p_hat = 1.0 - cur_p_hat;
// update
if (did_recall) p_hat[table_id] = (skill_pi1 * cur_p_hat + skill_mu * skill_pi0 * one_minus_cur_p_hat) / (skill_pi1 * cur_p_hat + skill_pi0 * one_minus_cur_p_hat);
else p_hat[table_id] = ((1.0 - skill_pi1) * cur_p_hat + skill_mu * (1.0 - skill_pi0) * one_minus_cur_p_hat) / ((1.0 - skill_pi1) * cur_p_hat + (1.0 - skill_pi0) * one_minus_cur_p_hat);
}
}
// ******************************************************************************************
// Converts a string reason for disabling a link pair into a struct data type
// ******************************************************************************************
DisabledReason disabledReasonFromString( const std::string& reason )
{
DisabledReason r;
try
{
r = reasonsFromString.at( reason );
}
catch( std::out_of_range )
{
r = USER;
}
return r;
}
static std::pair<double, double>
score(const boost::unordered_map<int, double> &real,
const boost::unordered_map<int, double> &fake,
double threshold = 0.0)
{
double res = 0.0;
int cnt = 0;
double e = std::numeric_limits<double>::epsilon();
for (auto i = fake.begin(); i != fake.end(); ++i)
if (i->second >= threshold &&
real.find(i->first) != real.end()) {
++cnt;
double p = real.at(i->first);
double q = fake.at(i->first);
if (p <= e)
res += (1-p) * log((1-p)/(1-q));
else if (1-p <= e || 1-q <= e)
res += p * log(p/q);
else res += p * log(p/q) + (1-p) * log((1-p)/(1-q));
}
return {res, cnt ? res / cnt : 0.0};
}
// ******************************************************************************************
// Converts a reason for disabling a link pair into a string
// ******************************************************************************************
const std::string disabledReasonToString( DisabledReason reason )
{
return reasonsToString.at( reason );
}
void timer_callback(const ros::TimerEvent &)
{
mil_msgs::MoveToResult actionresult;
// Handle disabled, killed, or no odom before attempting to produce trajectory
std::string err = "";
if (disabled)
err = "c3 disabled";
else if (kill_listener.isRaised())
err = "killed";
else if (!c3trajectory)
err = "no odom";
if (!err.empty())
{
if (c3trajectory)
c3trajectory.reset(); // On revive/enable, wait for odom before station keeping
// Cancel all goals while killed/disabled/no odom
if (actionserver.isNewGoalAvailable())
actionserver.acceptNewGoal();
if (actionserver.isActive())
{
actionresult.error = err;
actionresult.success = false;
actionserver.setAborted(actionresult);
}
return;
}
ros::Time now = ros::Time::now();
auto old_waypoint = current_waypoint;
if (actionserver.isNewGoalAvailable())
{
boost::shared_ptr<const mil_msgs::MoveToGoal> goal = actionserver.acceptNewGoal();
current_waypoint =
subjugator::C3Trajectory::Waypoint(Point_from_PoseTwist(goal->posetwist.pose, goal->posetwist.twist),
goal->speed, !goal->uncoordinated, !goal->blind);
current_waypoint_t = now;
this->linear_tolerance = goal->linear_tolerance;
this->angular_tolerance = goal->angular_tolerance;
waypoint_validity_.pub_size_ogrid(Pose_from_Waypoint(current_waypoint), (int)OGRID_COLOR::GREEN);
// Check if waypoint is valid
std::pair<bool, WAYPOINT_ERROR_TYPE> checkWPResult = waypoint_validity_.is_waypoint_valid(
Pose_from_Waypoint(current_waypoint), current_waypoint.do_waypoint_validation);
actionresult.error = WAYPOINT_ERROR_TO_STRING.at(checkWPResult.second);
actionresult.success = checkWPResult.first;
if (checkWPResult.first == false && waypoint_check_) // got a point that we should not move to
{
waypoint_validity_.pub_size_ogrid(Pose_from_Waypoint(current_waypoint), (int)OGRID_COLOR::RED);
if (checkWPResult.second ==
WAYPOINT_ERROR_TYPE::UNKNOWN) // if unknown, check if there's a huge displacement with the new waypoint
{
auto a_point = Pose_from_Waypoint(current_waypoint);
auto b_point = Pose_from_Waypoint(old_waypoint);
// If moved more than .5m, then don't allow
if (abs(a_point.position.x - b_point.position.x) > .5 || abs(a_point.position.y - b_point.position.y) > .5)
{
ROS_ERROR("can't move there! - need to rotate");
current_waypoint = old_waypoint;
}
}
// if point is occupied, reject move
if (checkWPResult.second == WAYPOINT_ERROR_TYPE::OCCUPIED)
{
ROS_ERROR("can't move there! - waypoint is occupied");
current_waypoint = old_waypoint;
}
// if point is above water, reject move
if (checkWPResult.second == WAYPOINT_ERROR_TYPE::ABOVE_WATER)
{
ROS_ERROR("can't move there! - waypoint is above water");
current_waypoint = old_waypoint;
}
if (checkWPResult.second == WAYPOINT_ERROR_TYPE::NO_OGRID)
{
ROS_ERROR("WaypointValidity - Did not recieve any ogrid");
}
}
}
if (actionserver.isPreemptRequested())
{
current_waypoint = c3trajectory->getCurrentPoint();
current_waypoint.do_waypoint_validation = false;
current_waypoint.r.qdot = subjugator::Vector6d::Zero(); // zero velocities
current_waypoint_t = now;
// don't try to make output c3 continuous when cancelled - instead stop as quickly as possible
c3trajectory.reset(new subjugator::C3Trajectory(current_waypoint.r, limits));
c3trajectory_t = now;
}
// Remember the previous trajectory
auto old_trajectory = c3trajectory->getCurrentPoint();
while (c3trajectory_t + traj_dt < now)
{
c3trajectory->update(traj_dt.toSec(), current_waypoint, (c3trajectory_t - current_waypoint_t).toSec());
c3trajectory_t += traj_dt;
}
// Check if we will hit something while in trajectory the new trajectory
geometry_msgs::Pose traj_point; // Convert messages to correct type
auto p = c3trajectory->getCurrentPoint();
traj_point.position = vec2xyz<Point>(p.q.head(3));
quaternionTFToMsg(tf::createQuaternionFromRPY(p.q[3], p.q[4], p.q[5]), traj_point.orientation);
std::pair<bool, WAYPOINT_ERROR_TYPE> checkWPResult =
waypoint_validity_.is_waypoint_valid(Pose_from_Waypoint(p), c3trajectory->do_waypoint_validation);
if (checkWPResult.first == false && checkWPResult.second == WAYPOINT_ERROR_TYPE::OCCUPIED && waypoint_check_)
{ // New trajectory will hit an occupied goal, so reject
ROS_ERROR("can't move there! - bad trajectory");
current_waypoint = old_trajectory;
current_waypoint.do_waypoint_validation = false;
current_waypoint.r.qdot = subjugator::Vector6d::Zero(); // zero velocities
current_waypoint_t = now;
c3trajectory.reset(new subjugator::C3Trajectory(current_waypoint.r, limits));
c3trajectory_t = now;
actionresult.success = false;
actionresult.error = WAYPOINT_ERROR_TO_STRING.at(WAYPOINT_ERROR_TYPE::OCCUPIED_TRAJECTORY);
}
PoseTwistStamped msg;
msg.header.stamp = c3trajectory_t;
msg.header.frame_id = fixed_frame;
msg.posetwist = PoseTwist_from_PointWithAcceleration(c3trajectory->getCurrentPoint());
trajectory_pub.publish(msg);
waypoint_validity_.pub_size_ogrid(Pose_from_Waypoint(c3trajectory->getCurrentPoint()), 200);
PoseStamped msgVis;
msgVis.header = msg.header;
msgVis.pose = msg.posetwist.pose;
trajectory_vis_pub.publish(msgVis);
PoseStamped posemsg;
posemsg.header.stamp = c3trajectory_t;
posemsg.header.frame_id = fixed_frame;
posemsg.pose = Pose_from_Waypoint(current_waypoint);
waypoint_pose_pub.publish(posemsg);
if (actionserver.isActive() &&
c3trajectory->getCurrentPoint().is_approximately(current_waypoint.r, max(1e-3, linear_tolerance),
max(1e-3, angular_tolerance)) &&
current_waypoint.r.qdot == subjugator::Vector6d::Zero())
{
actionresult.error = "";
actionresult.success = true;
actionserver.setSucceeded(actionresult);
}
}
