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