void QuadrotorPropulsion::update(double dt)
{
if (dt <= 0.0) return;
// std::cout << "u = [ ";
// for(std::size_t i = 0; i < propulsion_model_->u.size(); ++i)
// std::cout << propulsion_model_->u[i] << " ";
// std::cout << "]" << std::endl;
checknan(propulsion_model_->u, "propulsion model input");
checknan(propulsion_model_->x, "propulsion model state");
// update propulsion model
f(propulsion_model_->x.data(), propulsion_model_->u.data(), dt, propulsion_model_->y.data(), propulsion_model_->x_pred.data());
propulsion_model_->x = propulsion_model_->x_pred;
checknan(propulsion_model_->y, "propulsion model output");
// std::cout << "y = [ ";
// for(std::size_t i = 0; i < propulsion_model_->y.size(); ++i)
// std::cout << propulsion_model_->y[i] << " ";
// std::cout << "]" << std::endl;
wrench_.force.x = propulsion_model_->y[0];
wrench_.force.y = -propulsion_model_->y[1];
wrench_.force.z = propulsion_model_->y[2];
wrench_.torque.x = propulsion_model_->y[3];
wrench_.torque.y = -propulsion_model_->y[4];
wrench_.torque.z = -propulsion_model_->y[5];
motor_status_.voltage[0] = propulsion_model_->u[6];
motor_status_.voltage[1] = propulsion_model_->u[7];
motor_status_.voltage[2] = propulsion_model_->u[8];
motor_status_.voltage[3] = propulsion_model_->u[9];
motor_status_.frequency[0] = propulsion_model_->y[6];
motor_status_.frequency[1] = propulsion_model_->y[7];
motor_status_.frequency[2] = propulsion_model_->y[8];
motor_status_.frequency[3] = propulsion_model_->y[9];
motor_status_.running = motor_status_.frequency[0] > 1.0 && motor_status_.frequency[1] > 1.0 && motor_status_.frequency[2] > 1.0 && motor_status_.frequency[3] > 1.0;
motor_status_.current[0] = propulsion_model_->y[10];
motor_status_.current[1] = propulsion_model_->y[11];
motor_status_.current[2] = propulsion_model_->y[12];
motor_status_.current[3] = propulsion_model_->y[13];
}
double PID::getFilteredControlError(double& filtered_error, double time_constant, const ros::Duration& dt)
{
double dt_sec = dt.toSec();
filtered_error = checknan(filtered_error);
if (dt_sec + time_constant > 0.0) {
filtered_error = filtered_error + dt_sec * state_.p - (dt_sec / (dt_sec + time_constant)) * filtered_error;
}
return filtered_error;
}
void QuadrotorPropulsion::update(double dt)
{
if (dt <= 0.0) return;
ROS_DEBUG_STREAM_NAMED("quadrotor_propulsion", "propulsion.twist: " << PrintVector<double>(propulsion_model_->u.begin(), propulsion_model_->u.begin() + 6));
ROS_DEBUG_STREAM_NAMED("quadrotor_propulsion", "propulsion.voltage: " << PrintVector<double>(propulsion_model_->u.begin() + 6, propulsion_model_->u.begin() + 10));
ROS_DEBUG_STREAM_NAMED("quadrotor_propulsion", "propulsion.x_prior: " << PrintVector<double>(propulsion_model_->x.begin(), propulsion_model_->x.end()));
checknan(propulsion_model_->u, "propulsion model input");
checknan(propulsion_model_->x, "propulsion model state");
// update propulsion model
f(propulsion_model_->x.data(), propulsion_model_->u.data(), dt, propulsion_model_->y.data(), propulsion_model_->x_pred.data());
propulsion_model_->x = propulsion_model_->x_pred;
ROS_DEBUG_STREAM_NAMED("quadrotor_propulsion", "propulsion.x_post: " << PrintVector<double>(propulsion_model_->x.begin(), propulsion_model_->x.end()));
ROS_DEBUG_STREAM_NAMED("quadrotor_propulsion", "propulsion.force: " << PrintVector<double>(propulsion_model_->y.begin() + 0, propulsion_model_->y.begin() + 3) << " " <<
"propulsion.torque: " << PrintVector<double>(propulsion_model_->y.begin() + 3, propulsion_model_->y.begin() + 6));
checknan(propulsion_model_->y, "propulsion model output");
wrench_.force.x = propulsion_model_->y[0];
wrench_.force.y = -propulsion_model_->y[1];
wrench_.force.z = propulsion_model_->y[2];
wrench_.torque.x = propulsion_model_->y[3];
wrench_.torque.y = -propulsion_model_->y[4];
wrench_.torque.z = -propulsion_model_->y[5];
motor_status_.voltage[0] = propulsion_model_->u[6];
motor_status_.voltage[1] = propulsion_model_->u[7];
motor_status_.voltage[2] = propulsion_model_->u[8];
motor_status_.voltage[3] = propulsion_model_->u[9];
motor_status_.frequency[0] = propulsion_model_->y[6];
motor_status_.frequency[1] = propulsion_model_->y[7];
motor_status_.frequency[2] = propulsion_model_->y[8];
motor_status_.frequency[3] = propulsion_model_->y[9];
motor_status_.running = motor_status_.frequency[0] > 1.0 && motor_status_.frequency[1] > 1.0 && motor_status_.frequency[2] > 1.0 && motor_status_.frequency[3] > 1.0;
motor_status_.current[0] = propulsion_model_->y[10];
motor_status_.current[1] = propulsion_model_->y[11];
motor_status_.current[2] = propulsion_model_->y[12];
motor_status_.current[3] = propulsion_model_->y[13];
}
void QuadrotorAerodynamics::update(double dt)
{
if (dt <= 0.0) return;
boost::mutex::scoped_lock lock(mutex_);
// fill input vector u for drag model
drag_model_->u[0] = (twist_.linear.x - wind_.x);
drag_model_->u[1] = -(twist_.linear.y - wind_.y);
drag_model_->u[2] = -(twist_.linear.z - wind_.z);
drag_model_->u[3] = twist_.angular.x;
drag_model_->u[4] = -twist_.angular.y;
drag_model_->u[5] = -twist_.angular.z;
// We limit the input velocities to +-100. Required for numeric stability in case of collisions,
// where velocities returned by Gazebo can be very high.
limit(drag_model_->u, -100.0, 100.0);
// convert input to body coordinates
Eigen::Quaterniond orientation(orientation_.w, orientation_.x, orientation_.y, orientation_.z);
Eigen::Matrix<double,3,3> rotation_matrix(orientation.toRotationMatrix());
Eigen::Map<Eigen::Vector3d> linear(&(drag_model_->u[0]));
Eigen::Map<Eigen::Vector3d> angular(&(drag_model_->u[3]));
linear = rotation_matrix * linear;
angular = rotation_matrix * angular;
ROS_DEBUG_STREAM_NAMED("quadrotor_aerodynamics", "aerodynamics.twist: " << PrintVector<double>(drag_model_->u.begin(), drag_model_->u.begin() + 6));
checknan(drag_model_->u, "drag model input");
// update drag model
f(drag_model_->u.data(), dt, drag_model_->y.data());
ROS_DEBUG_STREAM_NAMED("quadrotor_aerodynamics", "aerodynamics.force: " << PrintVector<double>(drag_model_->y.begin() + 0, drag_model_->y.begin() + 3));
ROS_DEBUG_STREAM_NAMED("quadrotor_aerodynamics", "aerodynamics.torque: " << PrintVector<double>(drag_model_->y.begin() + 3, drag_model_->y.begin() + 6));
checknan(drag_model_->y, "drag model output");
// drag_model_ gives us inverted vectors!
wrench_.force.x = -( drag_model_->y[0]);
wrench_.force.y = -(-drag_model_->y[1]);
wrench_.force.z = -(-drag_model_->y[2]);
wrench_.torque.x = -( drag_model_->y[3]);
wrench_.torque.y = -(-drag_model_->y[4]);
wrench_.torque.z = -(-drag_model_->y[5]);
}
double PID::update(double error, double dx, const ros::Duration& dt)
{
if (!parameters_.enabled) return 0.0;
if (std::isnan(error)) return 0.0;
double dt_sec = dt.toSec();
// integral error
state_.i += error * dt_sec;
if (parameters_.limit_i > 0.0)
{
if (state_.i > parameters_.limit_i) state_.i = parameters_.limit_i;
if (state_.i < -parameters_.limit_i) state_.i = -parameters_.limit_i;
}
// differential error
if (dt_sec > 0.0 && !std::isnan(state_.p) && !std::isnan(state_.dx)) {
state_.d = (error - state_.p) / dt_sec + state_.dx - dx;
} else {
state_.d = -dx;
}
state_.dx = dx;
// proportional error
state_.p = error;
// calculate output...
double output = parameters_.k_p * state_.p + parameters_.k_i * state_.i + parameters_.k_d * state_.d;
int antiwindup = 0;
if (parameters_.limit_output > 0.0)
{
if (output > parameters_.limit_output) {
output = parameters_.limit_output;
antiwindup = 1;
}
if (output < -parameters_.limit_output) {
output = -parameters_.limit_output;
antiwindup = -1;
}
}
if (antiwindup && (error * dt_sec * antiwindup > 0.0)) state_.i -= error * dt_sec;
checknan(output);
return output;
}
////////////////////////////////////////////////////////////////////////////////
// Update the controller
void GazeboQuadrotorAerodynamics::Update()
{
Vector3 force, torque;
boost::mutex::scoped_lock lock(command_mutex_);
// Get simulator time
Time current_time = world->GetSimTime();
double dt = (current_time - last_time_).Double();
last_time_ = current_time;
if (dt <= 0.0) return;
// Get new commands/state
// callback_queue_.callAvailable();
while(1) {
if (!new_motor_voltages_.empty()) {
hector_uav_msgs::MotorPWMConstPtr new_motor_voltage = new_motor_voltages_.front();
Time new_time = Time(new_motor_voltage->header.stamp.sec, new_motor_voltage->header.stamp.nsec);
if (new_time == Time() || (new_time >= current_time - control_delay_ - control_tolerance_ && new_time <= current_time - control_delay_ + control_tolerance_)) {
motor_voltage_ = new_motor_voltage;
new_motor_voltages_.pop_front();
last_control_time_ = current_time - control_delay_;
ROS_DEBUG_STREAM("Using motor command valid at " << new_time << " for simulation step at " << current_time << " (dt = " << (current_time - new_time) << ")");
break;
} else if (new_time < current_time - control_delay_ - control_tolerance_) {
ROS_DEBUG("[quadrotor_aerodynamics] command received was too old: %f s", (new_time - current_time).Double());
new_motor_voltages_.pop_front();
continue;
}
}
if (new_motor_voltages_.empty() && motor_status_.on && control_period_ > 0 && current_time > last_control_time_ + control_period_ + control_tolerance_) {
ROS_WARN("[quadrotor_aerodynamics] waiting for command...");
if (command_condition_.timed_wait(lock, (ros::Duration(control_period_.sec, control_period_.nsec) * 100.0).toBoost())) continue;
ROS_ERROR("[quadrotor_aerodynamics] command timed out.");
motor_status_.on = false;
}
break;
}
// fill input vector u for propulsion model
velocity = model->GetRelativeLinearVel();
rate = model->GetRelativeAngularVel();
propulsion_model_->u[0] = velocity.x;
propulsion_model_->u[1] = -velocity.y;
propulsion_model_->u[2] = -velocity.z;
propulsion_model_->u[3] = rate.x;
propulsion_model_->u[4] = -rate.y;
propulsion_model_->u[5] = -rate.z;
if (motor_voltage_ && motor_voltage_->pwm.size() >= 4) {
propulsion_model_->u[6] = motor_voltage_->pwm[0] * 14.8 / 255.0;
propulsion_model_->u[7] = motor_voltage_->pwm[1] * 14.8 / 255.0;
propulsion_model_->u[8] = motor_voltage_->pwm[2] * 14.8 / 255.0;
propulsion_model_->u[9] = motor_voltage_->pwm[3] * 14.8 / 255.0;
} else {
propulsion_model_->u[6] = 0.0;
propulsion_model_->u[7] = 0.0;
propulsion_model_->u[8] = 0.0;
propulsion_model_->u[9] = 0.0;
}
// std::cout << "u = [ ";
// for(std::size_t i = 0; i < propulsion_model_->u.size(); ++i)
// std::cout << propulsion_model_->u[i] << " ";
// std::cout << "]" << std::endl;
checknan(propulsion_model_->u, "propulsion model input");
checknan(propulsion_model_->x, "propulsion model state");
// update propulsion model
quadrotorPropulsion(propulsion_model_->x.data(), propulsion_model_->u.data(), propulsion_model_->parameters_, dt, propulsion_model_->y.data(), propulsion_model_->x_pred.data());
propulsion_model_->x = propulsion_model_->x_pred;
force = force + checknan(Vector3(propulsion_model_->y[0], -propulsion_model_->y[1], propulsion_model_->y[2]), "propulsion model force");
torque = torque + checknan(Vector3(propulsion_model_->y[3], -propulsion_model_->y[4], -propulsion_model_->y[5]), "propulsion model torque");
// std::cout << "y = [ ";
// for(std::size_t i = 0; i < propulsion_model_->y.size(); ++i)
// std::cout << propulsion_model_->y[i] << " ";
// std::cout << "]" << std::endl;
// publish wrench
if (wrench_publisher_) {
wrench_.force.x = force.x;
wrench_.force.y = force.y;
wrench_.force.z = force.z;
wrench_.torque.x = torque.x;
wrench_.torque.y = torque.y;
wrench_.torque.z = torque.z;
wrench_publisher_.publish(wrench_);
}
// fill input vector u for drag model
drag_model_->u[0] = (velocity.x - wind_.x);
drag_model_->u[1] = -(velocity.y - wind_.y);
drag_model_->u[2] = -(velocity.z - wind_.z);
drag_model_->u[3] = rate.x;
drag_model_->u[4] = -rate.y;
drag_model_->u[5] = -rate.z;
// std::cout << "u = [ ";
// for(std::size_t i = 0; i < drag_model_->u.size(); ++i)
// std::cout << drag_model_->u[i] << " ";
// std::cout << "]" << std::endl;
checknan(drag_model_->u, "drag model input");
// update drag model
quadrotorDrag(drag_model_->u.data(), drag_model_->parameters_, dt, drag_model_->y.data());
force = force - checknan(Vector3(drag_model_->y[0], -drag_model_->y[1], -drag_model_->y[2]), "drag model force");
torque = torque - checknan(Vector3(drag_model_->y[3], -drag_model_->y[4], -drag_model_->y[5]), "drag model torque");
if (motor_status_publisher_ && current_time >= last_motor_status_time_ + control_period_) {
motor_status_.header.stamp = ros::Time(current_time.sec, current_time.nsec);
motor_status_.running = propulsion_model_->x[0] > 0.0 && propulsion_model_->x[1] > 0.0 && propulsion_model_->x[2] > 0.0 && propulsion_model_->x[3] > 0.0;
motor_status_.frequency.resize(4);
motor_status_.frequency[0] = propulsion_model_->x[0];
motor_status_.frequency[1] = propulsion_model_->x[1];
motor_status_.frequency[2] = propulsion_model_->x[2];
motor_status_.frequency[3] = propulsion_model_->x[3];
motor_status_publisher_.publish(motor_status_);
last_motor_status_time_ = current_time;
}
// set force and torque in gazebo
model->GetLink()->AddRelativeForce(force);
model->GetLink()->AddRelativeTorque(torque);
}
void plotylms()
{
gStyle->SetOptStat(1000000001);
TIter gdiriter(gDirectory->GetListOfKeys());
TObject *cur;
char *buf;
char suff[500];
char lbuf[500];
char nbuf[500];
suff[0] = '\0';
while (cur = gdiriter()) {
buf = cur->GetName();
if (strstr(buf, "CfnReYlm00")) {
cout << "Found suffix " << buf+10 << endl;
strcpy(suff, buf+10);
break;
}
}
if (!suff[0]) {
cout << "Did not find suffix" << endl;
return;
}
int maxl = 0;
gdiriter->Reset();
while (cur = gdiriter()) {
buf = cur->GetName();
if ((strstr(buf, "CfnReYlm")) && (strstr(buf, suff))){
cout << "Found l " << buf+8 << endl;
strncpy(lbuf, buf+8,1);
lbuf[1] = '\0';
int lcur = atoi(lbuf);
cout << "Found l " << lcur << endl;
if (lcur > maxl) maxl = lcur;
}
}
cout << "Maximum l " << maxl << endl;
int ilmcount= 0;
TH1D **cfnsr;
cfnsr = malloc(sizeof(TH1D *) * (maxl+1)*(maxl+1));
TH1D **cfnsi;
cfnsi = malloc(sizeof(TH1D *) * (maxl+1)*(maxl+1));
for (int il=0; il<=maxl; il++)
for (int im=0; im<=il; im++) {
sprintf(nbuf, "CfnReYlm%i%i%s", il, im, suff);
cfnsr[ilmcount] = new TH1D(*((TH1D *) gDirectory->Get(nbuf)));
checknan(cfnsr[ilmcount]);
sprintf(nbuf, "CfnImYlm%i%i%s", il, im, suff);
cfnsi[ilmcount] = new TH1D(*((TH1D *) gDirectory->Get(nbuf)));
checknan(cfnsi[ilmcount]);
ilmcount++;
}
TLine *l1 = new TLine(0.0, 1.0, cfnsr[0]->GetXaxis()->GetXmax(), 1.0);
l1->SetLineColor(14);
l1->SetLineStyle(2);
TLine *l0 = new TLine(0.0, 0.0, cfnsr[0]->GetXaxis()->GetXmax(), 0.0);
l0->SetLineColor(14);
l0->SetLineStyle(2);
TCanvas *canylm = new TCanvas("canylm","canylm",1600,800);
gPad->SetFillColor(0);
gPad->SetFillStyle(4000);
canylm->Divide(5, 2, 0.0001, 0.0001);
for (int ilm=0; ilm<(maxl+1)*(maxl+2)/2; ilm++) {
cout << "Drawing " << ilm << endl;
canylm->cd(ilm+1);
gPad->SetFillColor(0);
gPad->SetFillStyle(4000);
gPad->SetTopMargin(0.01);
gPad->SetRightMargin(0.01);
cfnsr[ilm]->SetTitle("");
cfnsr[ilm]->SetMarkerColor(2);
cfnsr[ilm]->SetMarkerSize(0.8);
cfnsr[ilm]->SetMarkerStyle(8);
cfnsr[ilm]->Draw();
cout << "Drawn " << ilm << endl;
cfnsi[ilm]->SetTitle("");
cfnsi[ilm]->SetMarkerColor(4);
cfnsi[ilm]->SetMarkerSize(0.8);
cfnsi[ilm]->SetMarkerStyle(8);
cfnsi[ilm]->Draw("SAME");
cout << "Drawn " << ilm << endl;
if (ilm)
l0->Draw();
else
l1->Draw();
}
TLatex lat;
lat.SetTextSize(0.12);
TCanvas *canylms = new TCanvas("canylms","canylms",1100,900);
gPad->SetFillColor(0);
gPad->SetFillStyle(4000);
canylms->Divide(1, 4, 0.0001, 0.0001);
int el = 0;
int em = 0;
for (int ilm=0; ilm<(maxl+1)*(maxl+2)/2; ilm++) {
TPad *curpad;
canylms->cd(el+1);
curpad = gPad;
cout << "Drawing " << ilm << " " << el << " " << em << " " << curpad << endl;
if (em == 0) {
if (el==0)
gPad->Divide(2,1,0.0001,0.0001);
if (el == 1)
gPad->Divide(2,1,0.0001,0.0001);
if (el == 2)
gPad->Divide(3,1,0.0001,0.0001);
if (el == 3)
gPad->Divide(4,1,0.0001,0.0001);
if ((el==0) && (em==0)) {
curpad->cd(2);
lat.DrawLatex(0.2, 0.8, "C_{0}^{0}");
lat.DrawLatex(0.2, 0.6, "C_{1}^{0}");
lat.DrawLatex(0.5, 0.6, "C_{1}^{1}");
lat.DrawLatex(0.2, 0.4, "C_{2}^{0}");
lat.DrawLatex(0.4, 0.4, "C_{2}^{1}");
lat.DrawLatex(0.6, 0.4, "C_{2}^{2}");
lat.DrawLatex(0.2, 0.2, "C_{3}^{0}");
lat.DrawLatex(0.35, 0.2, "C_{3}^{1}");
lat.DrawLatex(0.5, 0.2, "C_{3}^{2}");
lat.DrawLatex(0.65, 0.2, "C_{3}^{3}");
lat.SetTextColor(2);
lat.DrawLatex(0.35, 0.8, "real part");
lat.SetTextColor(4);
lat.DrawLatex(0.6, 0.8, "imaginary part");
}
}
cout << "Drawing " << ilm << " " << curpad << endl;
curpad->cd(em+1);
// canylms->cd(ilm+1);
gPad->SetFillColor(0);
gPad->SetFillStyle(4000);
gPad->SetTopMargin(0.01);
gPad->SetRightMargin(0.01);
cfnsr[ilm]->SetTitle("");
cfnsr[ilm]->SetMarkerColor(2);
cfnsr[ilm]->SetMarkerSize(0.8);
cfnsr[ilm]->SetMarkerStyle(8);
cfnsr[ilm]->Draw();
cout << "Drawn " << ilm << endl;
cfnsi[ilm]->SetTitle("");
cfnsi[ilm]->SetMarkerColor(4);
cfnsi[ilm]->SetMarkerSize(0.8);
cfnsi[ilm]->SetMarkerStyle(8);
cfnsi[ilm]->Draw("SAME");
cout << "Drawn " << ilm << endl;
if (ilm)
l0->Draw();
else
l1->Draw();
em++;
if (em>el) {
el++;
em = 0;
}
}
// canylms->SaveAs("canylms.eps");
// canylms->SaveAs("canylms.png");
}
