Skip to content

Commit

Permalink
updated array & matrix access to the range-checked version
Browse files Browse the repository at this point in the history
  • Loading branch information
@klaxalk
klaxalk committed Apr 20, 2024
1 parent fa0d8b6 commit 5390e1a
Show file tree
Hide file tree
Showing 6 changed files with 98 additions and 98 deletions.
2 changes: 1 addition & 1 deletion include/mrs_multirotor_simulator/uav_system/controllers/attitude_controller.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -178,7 +178,7 @@ double AttitudeController::intrinsicBodyRateToHeadingRate(const Eigen::Matrix3d&

// create the angular velocity tensor
Eigen::Matrix3d W;
W << 0, -w[2], w[1], w[2], 0, -w[0], -w[1], w[0], 0;
W << 0, -w(2), w(1), w(2), 0, -w(0), -w(1), w(0), 0;

// R derivative
Eigen::Matrix3d R_d = R * W;
Expand Down
6 changes: 3 additions & 3 deletions include/mrs_multirotor_simulator/uav_system/controllers/position_controller.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -76,9 +76,9 @@ reference::VelocityHdg PositionController::getControlSignal(const MultirotorMode

reference::VelocityHdg output;

output.velocity[0] = pid_x_.update(pos_error[0], dt);
output.velocity[1] = pid_y_.update(pos_error[1], dt);
output.velocity[2] = pid_z_.update(pos_error[2], dt);
output.velocity(0) = pid_x_.update(pos_error(0), dt);
output.velocity(1) = pid_y_.update(pos_error(1), dt);
output.velocity(2) = pid_z_.update(pos_error(2), dt);

output.heading = reference.heading;

Expand Down
12 changes: 6 additions & 6 deletions include/mrs_multirotor_simulator/uav_system/controllers/velocity_controller.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -72,9 +72,9 @@ reference::AccelerationHdg VelocityController::getControlSignal(const Multirotor

reference::AccelerationHdg output;

output.acceleration[0] = pid_x_.update(vel_error[0], dt);
output.acceleration[1] = pid_y_.update(vel_error[1], dt);
output.acceleration[2] = pid_z_.update(vel_error[2], dt);
output.acceleration(0) = pid_x_.update(vel_error(0), dt);
output.acceleration(1) = pid_y_.update(vel_error(1), dt);
output.acceleration(2) = pid_z_.update(vel_error(2), dt);

output.heading = reference.heading;

Expand All @@ -92,9 +92,9 @@ reference::AccelerationHdgRate VelocityController::getControlSignal(const Multir

reference::AccelerationHdgRate output;

output.acceleration[0] = pid_x_.update(vel_error[0], dt);
output.acceleration[1] = pid_y_.update(vel_error[1], dt);
output.acceleration[2] = pid_z_.update(vel_error[2], dt);
output.acceleration(0) = pid_x_.update(vel_error(0), dt);
output.acceleration(1) = pid_y_.update(vel_error(1), dt);
output.acceleration(2) = pid_z_.update(vel_error(2), dt);

output.heading_rate = reference.heading_rate;

Expand Down
58 changes: 29 additions & 29 deletions include/mrs_multirotor_simulator/uav_system/multirotor_model.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -203,12 +203,12 @@ void MultirotorModel::initializeState(void) {
void MultirotorModel::updateInternalState(void) {

for (int i = 0; i < 3; i++) {
internal_state_[0 + i] = state_.x(i);
internal_state_[3 + i] = state_.v(i);
internal_state_[6 + i] = state_.R(i, 0);
internal_state_[9 + i] = state_.R(i, 1);
internal_state_[12 + i] = state_.R(i, 2);
internal_state_[15 + i] = state_.omega(i);
internal_state_.at(0 + i) = state_.x(i);
internal_state_.at(3 + i) = state_.v(i);
internal_state_.at(6 + i) = state_.R(i, 0);
internal_state_.at(9 + i) = state_.R(i, 1);
internal_state_.at(12 + i) = state_.R(i, 2);
internal_state_.at(15 + i) = state_.omega(i);
}
}

Expand All @@ -225,19 +225,19 @@ void MultirotorModel::step(const double& dt) {
odeint::integrate_n_steps(rk, boost::ref(*this), internal_state_, 0.0, dt, 1);

for (int i = 0; i < N_INTERNAL_STATES; ++i) {
if (std::isnan(internal_state_[i])) {
if (std::isnan(internal_state_.at(i))) {
internal_state_ = save;
break;
}
}

for (int i = 0; i < 3; i++) {
state_.x(i) = internal_state_[0 + i];
state_.v(i) = internal_state_[3 + i];
state_.R(i, 0) = internal_state_[6 + i];
state_.R(i, 1) = internal_state_[9 + i];
state_.R(i, 2) = internal_state_[12 + i];
state_.omega(i) = internal_state_[15 + i];
state_.x(i) = internal_state_.at(0 + i);
state_.v(i) = internal_state_.at(3 + i);
state_.R(i, 0) = internal_state_.at(6 + i);
state_.R(i, 1) = internal_state_.at(9 + i);
state_.R(i, 2) = internal_state_.at(12 + i);
state_.omega(i) = internal_state_.at(15 + i);
}

double filter_const = exp((-dt) / (params_.motor_time_constant));
Expand Down Expand Up @@ -265,8 +265,8 @@ void MultirotorModel::step(const double& dt) {
const double hover_rpm = sqrt((params_.mass * params_.g) / (params_.n_motors * params_.kf));
if (input_.mean() <= 0.90 * hover_rpm) {

if (state_.x(2) < _initial_pos_[2] && state_.v(2) < 0) {
state_.x(2) = _initial_pos_[2];
if (state_.x(2) < _initial_pos_(2) && state_.v(2) < 0) {
state_.x(2) = _initial_pos_(2);
state_.v = Eigen::Vector3d::Zero();
state_.omega = Eigen::Vector3d::Zero();
}
Expand Down Expand Up @@ -302,12 +302,12 @@ void MultirotorModel::operator()(const MultirotorModel::InternalState& x, Multir
State cur_state;

for (int i = 0; i < 3; i++) {
cur_state.x(i) = x[0 + i];
cur_state.v(i) = x[3 + i];
cur_state.R(i, 0) = x[6 + i];
cur_state.R(i, 1) = x[9 + i];
cur_state.R(i, 2) = x[12 + i];
cur_state.omega(i) = x[15 + i];
cur_state.x(i) = x.at(0 + i);
cur_state.v(i) = x.at(3 + i);
cur_state.R(i, 0) = x.at(6 + i);
cur_state.R(i, 1) = x.at(9 + i);
cur_state.R(i, 2) = x.at(12 + i);
cur_state.omega(i) = x.at(15 + i);
}

Eigen::LLT<Eigen::Matrix3d> llt(cur_state.R.transpose() * cur_state.R);
Expand Down Expand Up @@ -349,17 +349,17 @@ void MultirotorModel::operator()(const MultirotorModel::InternalState& x, Multir
omega_dot = params_.J.inverse() * (torque_thrust.topRows(3) - cur_state.omega.cross(params_.J * cur_state.omega) + external_moment_);

for (int i = 0; i < 3; i++) {
dxdt[0 + i] = x_dot(i);
dxdt[3 + i] = v_dot(i);
dxdt[6 + i] = R_dot(i, 0);
dxdt[9 + i] = R_dot(i, 1);
dxdt[12 + i] = R_dot(i, 2);
dxdt[15 + i] = omega_dot(i);
dxdt.at(0 + i) = x_dot(i);
dxdt.at(3 + i) = v_dot(i);
dxdt.at(6 + i) = R_dot(i, 0);
dxdt.at(9 + i) = R_dot(i, 1);
dxdt.at(12 + i) = R_dot(i, 2);
dxdt.at(15 + i) = omega_dot(i);
}

for (int i = 0; i < N_INTERNAL_STATES; ++i) {
if (std::isnan(dxdt[i])) {
dxdt[i] = 0;
if (std::isnan(dxdt.at(i))) {
dxdt.at(i) = 0;
}
}
}
Expand Down
34 changes: 17 additions & 17 deletions src/multirotor_simulator.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -149,7 +149,7 @@ void MultirotorSimulator::onInit() {

for (size_t i = 0; i < uav_names.size(); i++) {

std::string uav_name = uav_names[i];
std::string uav_name = uav_names.at(i);

ROS_INFO("[MultirotorSimulator]: initializing '%s'", uav_name.c_str());

Expand Down Expand Up @@ -209,7 +209,7 @@ void MultirotorSimulator::timerMain([[maybe_unused]] const ros::WallTimerEvent&
sim_time_ = sim_time_ + ros::Duration(simulation_step_size);

for (size_t i = 0; i < uavs_.size(); i++) {
uavs_[i]->makeStep(simulation_step_size);
uavs_.at(i)->makeStep(simulation_step_size);
}

publishPoses();
Expand Down Expand Up @@ -303,7 +303,7 @@ void MultirotorSimulator::handleCollisions(void) {
std::vector<Eigen::VectorXd> poses;

for (size_t i = 0; i < uavs_.size(); i++) {
poses.push_back(uavs_[i]->getPose());
poses.push_back(uavs_.at(i)->getPose());
}

typedef KDTreeVectorOfVectorsAdaptor<my_vector_of_vectors_t, double> my_kd_tree_t;
Expand All @@ -320,41 +320,41 @@ void MultirotorSimulator::handleCollisions(void) {

for (size_t i = 0; i < uavs_.size(); i++) {

MultirotorModel::State state_1 = uavs_[i]->getState();
MultirotorModel::ModelParams params_1 = uavs_[i]->getParams();
MultirotorModel::State state_1 = uavs_.at(i)->getState();
MultirotorModel::ModelParams params_1 = uavs_.at(i)->getParams();

nanoflann::RadiusResultSet<double, int> resultSet(3.0, indices_dists);

mat_index.index->findNeighbors(resultSet, &state_1.x[0]);
mat_index.index->findNeighbors(resultSet, &state_1.x(0));

for (size_t j = 0; j < resultSet.m_indices_dists.size(); j++) {

const size_t idx = resultSet.m_indices_dists[j].first;
const double dist = resultSet.m_indices_dists[j].second;
const size_t idx = resultSet.m_indices_dists.at(j).first;
const double dist = resultSet.m_indices_dists.at(j).second;

if (idx == i) {
continue;
}

MultirotorModel::State state_2 = uavs_[idx]->getState();
MultirotorModel::ModelParams params_2 = uavs_[idx]->getParams();
MultirotorModel::State state_2 = uavs_.at(idx)->getState();
MultirotorModel::ModelParams params_2 = uavs_.at(idx)->getParams();

const double crit_dist = params_1.arm_length + params_1.prop_radius + params_2.arm_length + params_2.prop_radius;

const Eigen::Vector3d rel_pos = state_1.x - state_2.x;

if (dist < crit_dist) {
if (drs_params.collisions_crash) {
uavs_[idx]->crash();
uavs_.at(idx)->crash();
} else {
forces[i] += drs_params.collisions_rebounce * rel_pos.normalized() * params_1.mass * (params_2.mass / (params_1.mass + params_2.mass));
forces.at(i) += drs_params.collisions_rebounce * rel_pos.normalized() * params_1.mass * (params_2.mass / (params_1.mass + params_2.mass));
}
}
}
}

for (size_t i = 0; i < uavs_.size(); i++) {
uavs_[i]->applyForce(forces[i]);
uavs_.at(i)->applyForce(forces.at(i));
}
}

Expand All @@ -373,13 +373,13 @@ void MultirotorSimulator::publishPoses(void) {

for (size_t i = 0; i < uavs_.size(); i++) {

auto state = uavs_[i]->getState();
auto state = uavs_.at(i)->getState();

geometry_msgs::Pose pose;

pose.position.x = state.x[0];
pose.position.y = state.x[1];
pose.position.z = state.x[2];
pose.position.x = state.x(0);
pose.position.y = state.x(1);
pose.position.z = state.x(2);
pose.orientation = mrs_lib::AttitudeConverter(state.R);

pose_array.poses.push_back(pose);
Expand Down
Loading

0 comments on commit 5390e1a

Please sign in to comment.