void Xwings::draw(Modello *model){
if(this->death == false){
glTranslated(this->getX(), this->getY(), this->getZ());
glScaled(dimX, dimY, dimZ);
glEnable(GL_LIGHTING);
glEnable(GL_NORMALIZE);
rolling();
glRotatef(angle,-1,1,0);
model->prepare(1);
glDisable(GL_NORMALIZE);
glDisable(GL_LIGHTING);
}
}
void compute(RideItem *item, Specification spec, const QHash<QString,RideMetric*> &) {
// no ride or no samples
if (spec.isEmpty(item->ride()) || item->ride()->recIntSecs() == 0) {
setValue(RideFile::NIL);
setCount(0);
return;
}
int rollingwindowsize = 30 / item->ride()->recIntSecs();
double total = 0;
int count = 0;
// no point doing a rolling average if the
// sample rate is greater than the rolling average
// window!!
if (rollingwindowsize > 1) {
QVector<double> rolling(rollingwindowsize);
int index = 0;
double sum = 0;
// loop over the data and convert to a rolling
// average for the given windowsize
RideFileIterator it(item->ride(), spec);
while (it.hasNext()) {
struct RideFilePoint *point = it.next();
sum += point->watts;
sum -= rolling[index];
rolling[index] = point->watts;
total += pow(sum/rollingwindowsize,4); // raise rolling average to 4th power
count ++;
// move index on/round
index = (index >= rollingwindowsize-1) ? 0 : index+1;
}
}
if (count) {
np = pow(total / (count), 0.25);
secs = count * item->ride()->recIntSecs();
} else {
np = secs = 0;
}
setValue(np);
setCount(secs);
}
void compute(const RideFile *ride, const Zones *, int,
const HrZones *, int,
const QHash<QString,RideMetric*> &,
const Context *) {
if(ride->recIntSecs() == 0) return;
int rollingwindowsize = 30 / ride->recIntSecs();
double total = 0;
int count = 0;
// no point doing a rolling average if the
// sample rate is greater than the rolling average
// window!!
if (rollingwindowsize > 1) {
QVector<double> rolling(rollingwindowsize);
int index = 0;
double sum = 0;
// loop over the data and convert to a rolling
// average for the given windowsize
for (int i=0; i<ride->dataPoints().size(); i++) {
sum += ride->dataPoints()[i]->watts;
sum -= rolling[index];
rolling[index] = ride->dataPoints()[i]->watts;
total += pow(sum/rollingwindowsize,4); // raise rolling average to 4th power
count ++;
// move index on/round
index = (index >= rollingwindowsize-1) ? 0 : index+1;
}
}
if (count) {
np = pow(total / (count), 0.25);
secs = count * ride->recIntSecs();
} else {
np = secs = 0;
}
setValue(np);
setCount(secs);
}
void executeCB(const lasa_action_planners::PLAN2CTRLGoalConstPtr &goal)
{
std::string desired_action = goal->action_type;
ROS_INFO_STREAM( "Desired Action is " << desired_action);
isOkay = false, isFTOkay = false;
ros::Rate r(10);
ROS_INFO("Waiting for EE pose/ft topic...");
while(ros::ok() && (!isOkay || !isFTOkay)) {
r.sleep();
}
if(!ros::ok()) {
result_.success = 0;
ROS_INFO("%s: Failed", action_name_.c_str());
as_.setAborted(result_);
return;
}
// initialize action progress as null
feedback_.progress = 0;
bool success = false;
///////////////////////////////////////////////
/////----- EXECUTE REQUESTED ACTION ------/////
///////////////////////////////////////////////
if (goal->action_type=="HOME"){
// TODO: do something meaningful here
tf::Pose pose(ee_pose);
success = go_home(pose);
} else if(goal->action_type == "HOME_POUR") {
// Home for pouring with lasa robot
tf::Pose pose(ee_pose);
pose.setOrigin(tf::Vector3(-0.65, -0.11, 0.474));
pose.setRotation(tf::Quaternion(-0.006, 0.907, -0.420, -0.114));
success = go_home(pose);
}
if(goal->action_type=="FIND_TABLE"){
// Go down until table found
tf::Pose pose(ee_pose);
success = go_home(ee_pose);
if(success) {
success = find_table_for_rolling(goal->height, 0.03, goal->threshold);
}
}
if(goal->action_type=="ROLL_TEST"){
/**** For now use this instead **/
tf::Pose p(ee_pose);
/*********************************/
success = go_home(p);
if(success) {
success = find_table_for_rolling(0.15, 0.03, 5);
if(success) {
success = rolling(goal->force, goal->speed, 0.1);
}
}
}
// Use learned models to do shit
if(goal->action_type=="LEARNED_MODEL"){
DoughTaskPhase phase;
if(goal->action_name == "roll") {
phase = PHASEROLL;
} else if(goal->action_name == "reach") {
phase = PHASEREACH;
} else if(goal->action_name == "back") {
phase = PHASEBACK;
} else if(goal->action_name == "home") {
phase = PHASEHOME;
} else {
ROS_ERROR_STREAM("Unidentified action name "<<goal->action_name.c_str());
result_.success = 0;
as_.setAborted(result_);
return;
}
//TODO: update these from action
double reachingThreshold = 0.02, model_dt = 0.01;
//CDSController::DynamicsType masterType = CDSController::LINEAR_DYNAMICS;
CDSController::DynamicsType masterType = CDSController::MODEL_DYNAMICS;
//CDSController::DynamicsType slaveType = CDSController::LINEAR_DYNAMICS;
CDSController::DynamicsType slaveType = CDSController::UTHETA;
tf::Transform trans_obj, trans_att;
//Little hack for using reaching phase for HOMING
if (phase==PHASEHOME){
phase=PHASEREACH;
homing = true;
}
switch (action_mode) {
case ACTION_BOXY_FIXED:
/*if(phase == PHASEREACH) {
trans_obj.setOrigin(tf::Vector3(0.7, -0.43, 0.64)); // TODO: remember to add 0.15 to tf values as well
trans_obj.setRotation(tf::Quaternion(-0.01, 0.99, 0.005, -0.009));
} else if(phase == PHASEROLL) {
trans_obj.setOrigin(tf::Vector3(0.85, -0.43, ee_pose.getOrigin().z()));
trans_obj.setRotation(tf::Quaternion(-0.01, 0.99, 0.005, -0.009));
} else if(phase == PHASEBACK) {
trans_obj.setOrigin(tf::Vector3(0.73, -0.63, 0.84));
trans_obj.setRotation(tf::Quaternion(-0.01, 0.99, 0.005, -0.009));
}
trans_att.setIdentity();*/
trans_obj.setIdentity();
trans_obj.setOrigin(tf::Vector3(0.8, -0.43, 0.64));
if(phase == PHASEREACH) {
trans_att.setOrigin(tf::Vector3(-0.05, 0,0)); // TODO: remember to add 0.15 to tf values as well
trans_att.setRotation(tf::Quaternion(-0.01, 0.99, 0.005, -0.009));
} else if(phase == PHASEROLL) {
trans_att.setOrigin(tf::Vector3(0.05, 0, 0));
trans_att.setRotation(tf::Quaternion(-0.01, 0.99, 0.005, -0.009));
} else if(phase == PHASEBACK) {
trans_att.setOrigin(tf::Vector3(-0.15, -0.2, 0.15));
trans_att.setRotation(tf::Quaternion(-0.01, 0.99, 0.005, -0.009));
}
break;
case ACTION_LASA_FIXED:
if(phase == PHASEREACH) {
trans_obj.setOrigin(tf::Vector3(-0.55, -0.10, 0.3)); // TODO: remember to add 0.15 to tf values as well (z was 0.15)
trans_obj.setRotation(tf::Quaternion(0.0, 1.0, 0.0, 0.0));
} else if(phase == PHASEROLL) {
trans_obj.setOrigin(tf::Vector3(-0.55, -0.25, ee_pose.getOrigin().z()));
trans_obj.setRotation(tf::Quaternion(0.0, 1.0, 0.0, 0.0));
} else if(phase == PHASEBACK) {
trans_obj.setOrigin(tf::Vector3(-0.55, -0.25, 0.3)); //(z was 0.15)
trans_obj.setRotation(tf::Quaternion(0.0, 1.0, 0.0, 0.0));
}
trans_att.setIdentity();
break;
case ACTION_VISION:
trans_obj.setRotation(tf::Quaternion(goal->object_frame.rotation.x,goal->object_frame.rotation.y,
goal->object_frame.rotation.z,goal->object_frame.rotation.w));
trans_obj.setOrigin(tf::Vector3(goal->object_frame.translation.x, goal->object_frame.translation.y,
goal->object_frame.translation.z));
trans_att.setRotation(tf::Quaternion(goal->attractor_frame.rotation.x,goal->attractor_frame.rotation.y,
goal->attractor_frame.rotation.z,goal->attractor_frame.rotation.w));
trans_att.setOrigin(tf::Vector3(goal->attractor_frame.translation.x, goal->attractor_frame.translation.y,
goal->attractor_frame.translation.z));
if (bWaitForForces){ //HACK FOR BOXY
// Hack! For setting heights for reach and add offset of roller
if(phase == PHASEREACH) {
trans_att.setOrigin(tf::Vector3(goal->attractor_frame.translation.x, goal->attractor_frame.translation.y,goal->attractor_frame.translation.z + 0.1));
}
// Hack! safety for rolling
if(phase == PHASEROLL) {
trans_obj.setOrigin(tf::Vector3(goal->object_frame.translation.x, goal->object_frame.translation.y,ee_pose.getOrigin().getZ()));
trans_att.setOrigin(tf::Vector3(goal->attractor_frame.translation.x, goal->attractor_frame.translation.y, 0.0));
}
}
/*if (bWaitForForces){ //HACK FOR LASA
// Hack! For setting heights for reach and back
if(phase == PHASEREACH || phase == PHASEBACK) {
trans_obj.getOrigin().setZ(0.15);
trans_att.getOrigin().setZ(0.0);
}
// Hack! safety for rolling
if(phase == PHASEROLL) {
trans_obj.getOrigin().setZ(ee_pose.getOrigin().getZ());
trans_att.getOrigin().setZ(0.0);
}
}*/
break;
default:
break;
}
std::string force_gmm_id = goal->force_gmm_id;
success = learned_model_execution(phase, masterType, slaveType, reachingThreshold,
model_dt, trans_obj, trans_att, base_path, force_gmm_id);
}
result_.success = success;
homing=false;
if(success)
{
if (!homing){
ROS_WARN_STREAM("STORE PLOT");
plot_dyn = 2;
plot_dyn_msg.data = plot_dyn;
pub_plot_dyn_.publish(plot_dyn_msg);
ros::Rate r(1);
r.sleep();
}
//Wait for message of "plot stored"
/*ros::Rate wait(1000);
while(ros::ok() && (plot_published!=1)) {
ros::spinOnce();
wait.sleep();
}*/
ROS_INFO("%s: Succeeded", action_name_.c_str());
as_.setSucceeded(result_);
} else {
ROS_INFO("%s: Failed", action_name_.c_str());
as_.setAborted(result_);
}
}
