TranslationRotation3D::TranslationRotation3D(Eigen::Vector3d T,
Eigen::Vector3d R) {
double T_in[3];
double R_in[3];
Eigen::Map<Eigen::Vector3d> T_tmp(T_in);
Eigen::Map<Eigen::Vector3d> R_tmp(R_in);
T_tmp = T;
R_tmp = R;
setT(T_in);
setR(R_in);
}
void Standard::update_mc_state()
{
VtolType::update_mc_state();
// enable MC motors here in case we transitioned directly to MC mode
if (_flag_enable_mc_motors) {
set_max_mc(2000);
set_idle_mc();
_flag_enable_mc_motors = false;
}
// if the thrust scale param is zero or the drone is on manual mode,
// then the pusher-for-pitch strategy is disabled and we can return
if (_params_standard.forward_thrust_scale < FLT_EPSILON ||
!_v_control_mode->flag_control_position_enabled) {
return;
}
// Do not engage pusher assist during a failsafe event
// There could be a problem with the fixed wing drive
if (_attc->get_vtol_vehicle_status()->vtol_transition_failsafe) {
return;
}
// disable pusher assist during landing
if (_attc->get_pos_sp_triplet()->current.valid
&& _attc->get_pos_sp_triplet()->current.type == position_setpoint_s::SETPOINT_TYPE_LAND) {
return;
}
matrix::Dcmf R(matrix::Quatf(_v_att->q));
matrix::Dcmf R_sp(matrix::Quatf(_v_att_sp->q_d));
matrix::Eulerf euler(R);
matrix::Eulerf euler_sp(R_sp);
_pusher_throttle = 0.0f;
// direction of desired body z axis represented in earth frame
matrix::Vector3f body_z_sp(R_sp(0, 2), R_sp(1, 2), R_sp(2, 2));
// rotate desired body z axis into new frame which is rotated in z by the current
// heading of the vehicle. we refer to this as the heading frame.
matrix::Dcmf R_yaw = matrix::Eulerf(0.0f, 0.0f, -euler(2));
body_z_sp = R_yaw * body_z_sp;
body_z_sp.normalize();
// calculate the desired pitch seen in the heading frame
// this value corresponds to the amount the vehicle would try to pitch forward
float pitch_forward = atan2f(body_z_sp(0), body_z_sp(2));
// only allow pitching forward up to threshold, the rest of the desired
// forward acceleration will be compensated by the pusher
if (pitch_forward < -_params_standard.down_pitch_max) {
// desired roll angle in heading frame stays the same
float roll_new = -asinf(body_z_sp(1));
_pusher_throttle = (sinf(-pitch_forward) - sinf(_params_standard.down_pitch_max))
* _params_standard.forward_thrust_scale;
// return the vehicle to level position
float pitch_new = 0.0f;
// create corrected desired body z axis in heading frame
matrix::Dcmf R_tmp = matrix::Eulerf(roll_new, pitch_new, 0.0f);
matrix::Vector3f tilt_new(R_tmp(0, 2), R_tmp(1, 2), R_tmp(2, 2));
// rotate the vector into a new frame which is rotated in z by the desired heading
// with respect to the earh frame.
float yaw_error = _wrap_pi(euler_sp(2) - euler(2));
matrix::Dcmf R_yaw_correction = matrix::Eulerf(0.0f, 0.0f, -yaw_error);
tilt_new = R_yaw_correction * tilt_new;
// now extract roll and pitch setpoints
_v_att_sp->pitch_body = atan2f(tilt_new(0), tilt_new(2));
_v_att_sp->roll_body = -asinf(tilt_new(1));
R_sp = matrix::Eulerf(_v_att_sp->roll_body, _v_att_sp->pitch_body, euler_sp(2));
matrix::Quatf q_sp(R_sp);
q_sp.copyTo(_v_att_sp->q_d);
}
_pusher_throttle = _pusher_throttle < 0.0f ? 0.0f : _pusher_throttle;
}
void Standard::update_mc_state()
{
VtolType::update_mc_state();
// enable MC motors here in case we transitioned directly to MC mode
if (_flag_enable_mc_motors) {
set_max_mc(2000);
set_idle_mc();
_flag_enable_mc_motors = false;
}
// if the thrust scale param is zero then the pusher-for-pitch strategy is disabled and we can return
if (_params_standard.forward_thrust_scale < FLT_EPSILON) {
return;
}
matrix::Dcmf R(matrix::Quatf(_v_att->q));
matrix::Dcmf R_sp(&_v_att_sp->R_body[0]);
matrix::Eulerf euler(R);
matrix::Eulerf euler_sp(R_sp);
_pusher_throttle = 0.0f;
// direction of desired body z axis represented in earth frame
matrix::Vector3f body_z_sp(R_sp(0, 2), R_sp(1, 2), R_sp(2, 2));
// rotate desired body z axis into new frame which is rotated in z by the current
// heading of the vehicle. we refer to this as the heading frame.
matrix::Dcmf R_yaw = matrix::Eulerf(0.0f, 0.0f, -euler(2));
body_z_sp = R_yaw * body_z_sp;
body_z_sp.normalize();
// calculate the desired pitch seen in the heading frame
// this value corresponds to the amount the vehicle would try to pitch forward
float pitch_forward = asinf(body_z_sp(0));
// only allow pitching forward up to threshold, the rest of the desired
// forward acceleration will be compensated by the pusher
if (pitch_forward < -_params_standard.down_pitch_max) {
// desired roll angle in heading frame stays the same
float roll_new = -atan2f(body_z_sp(1), body_z_sp(2));
_pusher_throttle = (sinf(-pitch_forward) - sinf(_params_standard.down_pitch_max))
* _v_att_sp->thrust * _params_standard.forward_thrust_scale;
// limit desired pitch
float pitch_new = -_params_standard.down_pitch_max;
// create corrected desired body z axis in heading frame
matrix::Dcmf R_tmp = matrix::Eulerf(roll_new, pitch_new, 0.0f);
matrix::Vector3f tilt_new(R_tmp(0, 2), R_tmp(1, 2), R_tmp(2, 2));
// rotate the vector into a new frame which is rotated in z by the desired heading
// with respect to the earh frame.
float yaw_error = _wrap_pi(euler_sp(2) - euler(2));
matrix::Dcmf R_yaw_correction = matrix::Eulerf(0.0f, 0.0f, -yaw_error);
tilt_new = R_yaw_correction * tilt_new;
// now extract roll and pitch setpoints
float pitch = asinf(tilt_new(0));
float roll = -atan2f(tilt_new(1), tilt_new(2));
R_sp = matrix::Eulerf(roll, pitch, euler_sp(2));
matrix::Quatf q_sp(R_sp);
memcpy(&_v_att_sp->R_body[0], &R_sp._data[0], sizeof(_v_att_sp->R_body));
memcpy(&_v_att_sp->q_d[0], &q_sp._data[0], sizeof(_v_att_sp->q_d));
}
_pusher_throttle = _pusher_throttle < 0.0f ? 0.0f : _pusher_throttle;
}
void Verify::apply()
{
print_pre_info();
if(!_valid || _c == NULL)
{
std::cerr << "Verify not valid. Exiting" << std::endl;
return;
}
int steps = _strides;
int samples = _samples;
int N = _size;
if(_exp){
nr_links = stat::make_exponential_points_array( N, steps, START_POINT );
}else{
nr_links = stat::make_linear_points_array( N, steps, START_POINT );
}
link_mean = (nr_links.segment(0, steps) + nr_links.segment(1, steps)) / 2;
set_theoretical_values();
Eigen::ArrayXXd binned_chain = Eigen::ArrayXXd::Zero(steps,3*samples);
std::vector< std::vector<PFloat> > w = std::vector< std::vector<PFloat> >(steps);
for(int i = 0; i < steps; i++)
{
w[i] = std::vector<PFloat>(samples);
}
Eigen::ArrayXXd Rg_tmp = Eigen::ArrayXXd::Zero(steps,samples);
for(int i = 0; i < samples; i++)
{
_c->build(N);
// Vector containing the weight for each individual link
Eigen::VectorXd w_tmp = _c->weights();
// bin values
for(int j = 0; j<steps; j++)
{
Eigen::ArrayXXd sub_chain = _c->as_array( nr_links(j), nr_links(j+1)-nr_links(j) ).transpose();
binned_chain(j,3*i+0) = sub_chain.col(0).mean();
binned_chain(j,3*i+1) = sub_chain.col(1).mean();
binned_chain(j,3*i+2) = sub_chain.col(2).mean();
Rg_tmp(j,i) = _c->Rg(0,floor(link_mean(j)));
w[j][i] = mult_weights( w_tmp.segment(0, nr_links(j+1)) );
}
if(verbose)
{
write_to_terminal(N,i,steps);
}
}
Eigen::ArrayXXd R_tmp = Eigen::ArrayXXd::Zero(steps,samples);
R = Eigen::ArrayXd::Zero(steps);
R_var = Eigen::ArrayXd::Zero(steps);
Rg = Eigen::ArrayXd::Zero(steps);
Rg_var = Eigen::ArrayXd::Zero(steps);
for(int i = 0; i < steps; i++)
{
for(int j = 0; j < samples; j++)
{
Eigen::Vector3d pos = binned_chain.block(i,3*j,1,3).transpose();
R_tmp(i,j) = pos.norm();
}
}
for(int i = 0; i<steps; i++)
{
Eigen::Vector2d mv = stat::get_mean_and_variance(R_tmp.row(i), w[i]);
R(i) = mv(0);
R_var(i) = mv(1);
mv = stat::get_mean_and_variance(Rg_tmp.row(i), w[i]);
Rg(i) = mv(0);
Rg_var(i) = mv(1);
}
write_to_file();
print_post_info();
}
