inline void applyAngVelToState(TrackingSystem const &sys, BodyState &state,
BodyProcessModel &processModel,
CannedIMUMeasurement const &meas) {
Eigen::Vector3d angVel;
meas.restoreAngVel(angVel);
Eigen::Vector3d var;
meas.restoreAngVelVariance(var);
#if 0
static const double dt = 0.02;
/// Rotate it into camera space - it's bTb and we want cTc
/// @todo do this without rotating into camera space?
Eigen::Quaterniond cTb = state.getQuaternion();
// Eigen::Matrix3d bTc(state.getQuaternion().conjugate());
/// Turn it into an incremental quat to do the space transformation
Eigen::Quaterniond incrementalQuat =
cTb * angVelVecToIncRot(angVel, dt) * cTb.conjugate();
/// Then turn it back into an angular velocity vector
angVel = incRotToAngVelVec(incrementalQuat, dt);
// angVel = (getRotationMatrixToCameraSpace(sys) * angVel).eval();
/// @todo transform variance?
kalman::AngularVelocityMeasurement<BodyState> kalmanMeas{angVel, var};
kalman::correct(state, processModel, kalmanMeas);
#else
kalman::IMUAngVelMeasurement kalmanMeas{angVel, var};
auto correctionInProgress =
kalman::beginUnscentedCorrection(state, kalmanMeas);
auto outputMeas = [&] {
std::cout << "state: " << state.angularVelocity().transpose()
<< " and measurement: " << angVel.transpose();
};
if (!correctionInProgress.stateCorrectionFinite) {
std::cout
<< "Non-finite state correction in applying angular velocity: ";
outputMeas();
std::cout << "\n";
return;
}
if (!correctionInProgress.finishCorrection(true)) {
std::cout << "Non-finite error covariance after applying angular "
"velocity: ";
outputMeas();
std::cout << "\n";
}
#endif
}
inline void applyAngVelToState(TrackingSystem const &sys, BodyState &state,
BodyProcessModel &processModel,
CannedIMUMeasurement const &meas) {
Eigen::Vector3d angVel;
meas.restoreAngVel(angVel);
Eigen::Vector3d var;
meas.restoreAngVelVariance(var);
/// Rotate it into camera space - it's bTb and we want cTc
/// @todo do this without rotating into camera space?
Eigen::Quaterniond cTb = state.getQuaternion();
//Eigen::Matrix3d bTc(state.getQuaternion().conjugate());
Eigen::Quaterniond incrementalQuat = cTb * util::quat_exp_map(angVel).exp() *
cTb.conjugate();
angVel = util::quat_exp_map(incrementalQuat).ln();
// angVel = (getRotationMatrixToCameraSpace(sys) * angVel).eval();
/// @todo transform variance?
kalman::AngularVelocityMeasurement<BodyState> kalmanMeas{angVel, var};
kalman::correct(state, processModel, kalmanMeas);
}
void Transformer::transform() {
// convert translation and rotation from local to global coords
Eigen::Quaterniond refq = ref_.get_rotation();
Eigen::Vector3d global_t(refq * t_);
Eigen::Quaterniond global_q(pick_positive(refq * q_ * refq.conjugate()));
// get rb centroid as a translation
Eigen::Vector3d centroid(Referential(m_, target_).get_centroid());
// transform each rigid member
RigidBody d(m_, target_);
ParticleIndexes pis(d.get_member_particle_indexes());
for (unsigned i = 0; i < pis.size(); i++) {
XYZ xyz(m_, pis[i]);
Eigen::Vector3d coords;
coords << xyz.get_x(), xyz.get_y(), xyz.get_z();
Eigen::Vector3d newcoords = global_q * (coords - centroid) + centroid
+ global_t;
xyz.set_x(newcoords(0));
xyz.set_y(newcoords(1));
xyz.set_z(newcoords(2));
}
// update rigid body
d.set_reference_frame_from_members(pis);
}
void des_thrusters::step(double delta)
{
bool softkilled;
shm_get(switches, soft_kill, softkilled);
shm_getg(settings_control, shm_settings);
if (softkilled || !shm_settings.enabled) {
f -= f;
t -= t;
return;
}
// Read PID settings & desires.
// (would switching to watchers improve performance?)
SHM_GET_PID(settings_heading, shm_hp, hp)
SHM_GET_PID(settings_pitch, shm_pp, pp)
SHM_GET_PID(settings_roll, shm_rp, rp)
SHM_GET_PID(settings_velx, shm_xp, xp)
SHM_GET_PID(settings_vely, shm_yp, yp)
SHM_GET_PID(settings_depth, shm_dp, dp)
shm_getg(desires, shm_desires);
// Read current orientation, velocity & position (in the model frame).
// Orientation quaternion in the model frame.
Eigen::Quaterniond qm = q * mtob_rq;
Eigen::Vector3d rph = quat_to_euler(qm);
// Component of the model quaternion about the z-axis (heading-only rotation).
Eigen::Quaterniond qh = euler_to_quat(rph[2], 0, 0);
// Velocity in heading modified world space.
Eigen::Vector3d vp = qh.conjugate() * v;
// Step the PID loops.
Eigen::Vector3d desires = quat_to_euler(euler_to_quat(shm_desires.heading * DEG_TO_RAD,
shm_desires.pitch * DEG_TO_RAD,
shm_desires.roll * DEG_TO_RAD)) * RAD_TO_DEG;
double ho = hp.step(delta, desires[2], rph[2] * RAD_TO_DEG);
double po = pp.step(delta, desires[1], rph[1] * RAD_TO_DEG);
double ro = rp.step(delta, desires[0], rph[0] * RAD_TO_DEG);
double xo = xp.step(delta, shm_desires.speed, vp[0]);
double yo = yp.step(delta, shm_desires.sway_speed, vp[1]);
double zo = dp.step(delta, shm_desires.depth, x[2]);
// f is in the heading modified world frame.
// t is in the model frame.
// We will work with f and b in the model frame until the end of this
// function.
f[0] = shm_settings.velx_active ? xo : 0;
f[1] = shm_settings.vely_active ? yo : 0;
f[2] = shm_settings.depth_active ? zo : 0;
f *= m;
Eigen::Vector3d w_in;
w_in[0] = shm_settings.roll_active ? ro : 0;
w_in[1] = shm_settings.pitch_active ? po : 0;
w_in[2] = shm_settings.heading_active ? ho : 0;
// TODO Avoid this roundabout conversion from hpr frame
// to world frame and back to model frame.
Eigen::Quaterniond qhp = euler_to_quat(rph[2], rph[1], 0);
Eigen::Vector3d w = qm.conjugate() * (Eigen::Vector3d(0, 0, w_in[2]) +
qh * Eigen::Vector3d(0, w_in[1], 0) +
qhp * Eigen::Vector3d(w_in[0], 0, 0));
t = btom_rm * q.conjugate() * I * q * mtob_rm * w;
// Output diagnostic information. Shown by auv-control-helm.
SHM_PUT_PID(control_internal_heading, ho)
SHM_PUT_PID(control_internal_pitch, po)
SHM_PUT_PID(control_internal_roll, ro)
SHM_PUT_PID(control_internal_velx, xo)
SHM_PUT_PID(control_internal_vely, yo)
SHM_PUT_PID(control_internal_depth, zo)
// Subtract passive forces.
// (this implementation does not support discrimination!)
if (shm_settings.buoyancy_forces || shm_settings.drag_forces) {
// pff is in the body frame.
screw ps = pff();
f -= qh.conjugate() * q * ps.first;
t -= btom_rm * ps.second;
}
// Hyper-ellipsoid clamping. Sort of limiting to the maximum amount of
// energy the sub can output (e.g. can't move forward at full speed
// and pitch at full speed at the same time).
// Doesn't really account for real-world thruster configurations (e.g.
// it might be possible to move forward at full speed and ascend at
// full speed at the same time) but it's just an approximation.
for (int i = 0; i < 3; i++) {
f[i] /= axes[i];
}
for (int i = 0; i < 3; i++) {
t[i] /= axes[3 + i];
}
double m = f.dot(f) + t.dot(t);
if (m > 1) {
double sm = sqrt(m);
f /= sm;
t /= sm;
}
for (int i = 0; i < 3; i++) {
f[i] *= axes[i];
}
for (int i = 0; i < 3; i++) {
t[i] *= axes[3 + i];
}
// Regular min/max clamping.
clamp(f, fa, fb, 3);
clamp(t, ta, tb, 3);
f = q.conjugate() * qh * f;
t = mtob_rm * t;
}
bool GazeboInterface::readJointStates()
{
if(!ROSControlInterface::readJointStates())
return false;
if(m_last_modelStates)
{
std::vector<std::string>::const_iterator it = std::find(
m_last_modelStates->name.begin(), m_last_modelStates->name.end(),
m_modelName
);
if(it != m_last_modelStates->name.end())
{
int idx = it - m_last_modelStates->name.begin();
//
// Orientation feedback
//
// Retrieve the robot pose
const geometry_msgs::Pose& pose = m_last_modelStates->pose[idx];
// Set the robot orientation
Eigen::Quaterniond quat;
tf::quaternionMsgToEigen(pose.orientation, quat);
quat.normalize();
m_model->setRobotOrientation(quat);
// Provide /odom if wanted. Convention: /odom is on the floor.
if(m_publishOdom && m_last_modelStatesStamp - m_initTime > ros::Duration(3.0))
{
tf::StampedTransform trans;
trans.frame_id_ = "/odom";
trans.child_frame_id_ = "/ego_rot";
trans.stamp_ = m_last_modelStatesStamp;
trans.setIdentity();
tf::Vector3 translation;
tf::pointMsgToTF(pose.position, translation);
trans.setOrigin(translation);
Eigen::Quaterniond rot;
rot = Eigen::AngleAxisd(m_model->robotEYaw(), Eigen::Vector3d::UnitZ());
tf::Quaternion quat;
tf::quaternionEigenToTF(rot, quat);
trans.setRotation(quat);
ROS_DEBUG("robot pos: Z = %f, yaw: %f, stamp: %f", translation.z(), m_model->robotEYaw(), trans.stamp_.toSec());
ROS_DEBUG("robot pos tf: %f %f %f %f %f %f %f",
trans.getOrigin().x(), trans.getOrigin().y(), trans.getOrigin().z(),
trans.getRotation().w(), trans.getRotation().x(), trans.getRotation().y(), trans.getRotation().z()
);
m_pub_tf.sendTransform(trans);
}
//
// Angular velocity feedback
//
// Retrieve the robot twist
const geometry_msgs::Twist& twist = m_last_modelStates->twist[idx];
// Retrieve the robot angular velocity in global coordinates
Eigen::Vector3d globalAngularVelocity;
tf::vectorMsgToEigen(twist.angular, globalAngularVelocity);
// Set the measured angular velocity (local coordinates)
m_model->setRobotAngularVelocity(quat.conjugate() * globalAngularVelocity);
//
// Acceleration feedback
//
// We'd need an extra Gazebo plugin for acceleration sensing.
// For now we can do without and simply calculate the current
// acceleration due to gravity (neglect inertial accelerations).
Eigen::Vector3d globalGravityAcceleration(0.0, 0.0, 9.81);
// Set the measured acceleration (local coordinates)
m_model->setAccelerationVector(quat.conjugate() * globalGravityAcceleration);
//
// Magnetic field vector feedback
//
// We assume that north is positive X. The values of 0.20G (horiz)
// and 0.44G (vert) are approximately valid for central europe.
Eigen::Vector3d globalMagneticVector(0.20, 0.00, -0.44); // In gauss
// Set the measured magnetic field vector (local coordinates)
m_model->setMagneticFieldVector(quat.conjugate() * globalMagneticVector);
}
}
// Return success
return true;
}
void
EstimatorFull::UpdateCamera(const Eigen::Vector3d &p_c_v, const Eigen::Quaterniond &q_c_v,
const Eigen::Matrix<double,6,6> &R,
bool absolute, bool isKeyframe, double dLambda )
{
Eigen::QuaternionAd q_c_kf;
Eigen::Vector3d p_c_kf;
if (absolute)
{
q_c_kf = Eigen::QuaternionAd(q_c_v.conjugate() * q_kf_v.toQuat().conjugate());
p_c_kf = q_kf_v.toQuat().toRotationMatrix() * (p_c_v - p_kf_v);
}
else
{
q_c_kf.q = q_c_v;
p_c_kf = p_c_v;
}
Matrix3d C_q_w_v = q_w_v.toQuat().toRotationMatrix();
Matrix3d C_q_i_w = q_i_w.toQuat().toRotationMatrix();
Matrix3d C_q_c_i = q_c_i.toQuat().toRotationMatrix();
// Build Jacobian
Matrix<double,3,28> H_p;
H_p <<
C_q_w_v.transpose() * exp(lambda),
Matrix3d::Zero(),
-C_q_w_v.transpose()*C_q_i_w.transpose()*crossMat(p_c_i)*exp(lambda),
Matrix3d::Zero(),
Matrix3d::Zero(),
(C_q_w_v.transpose()*(C_q_i_w.transpose()*p_c_i + p_i_w) + p_w_v) * exp(lambda),
C_q_w_v.transpose()*C_q_i_w.transpose()*exp(lambda),
Matrix3d::Zero(),
Matrix3d::Identity()*exp(lambda),
-C_q_w_v.transpose()*crossMat( p_i_w + C_q_i_w.transpose()*p_c_i )*exp(lambda);
Matrix<double,3,28> H_q;
H_q <<
Matrix3d::Zero(),
Matrix3d::Zero(),
C_q_c_i,
Matrix3d::Zero(),
Matrix3d::Zero(),
Vector3d::Zero(),
Matrix3d::Zero(),
Matrix3d::Identity(),
Matrix3d::Zero(),
C_q_c_i*C_q_i_w;
Matrix<double,6,28> H;
H << H_p, H_q;
// Calculate residual
Vector3d r_p = p_c_kf - ( C_q_w_v.transpose()*(p_i_w + C_q_i_w.transpose()*p_c_i) + p_w_v ) * exp(lambda);
Quaterniond r_q = (q_c_kf.toQuat()*( q_c_i.toQuat()*q_i_w.toQuat()*q_w_v.toQuat() ).conjugate()).conjugate();
Matrix<double,6,1> r;
r << r_p, 2*r_q.x(), 2*r_q.y(), 2*r_q.z();
// Calculate kalmn gain
Matrix<double,6,6> S = H*P*H.transpose() + R;
//K = P*H.transpose()*S^-1;
//TODO: use cholsky to solve K*S = P*H.transposed()?
// (K*S)' = (P*H')'
// S'*K' = H*P'
// K' = S.transpose.ldlt().solve(H*P.transpose)
Matrix<double,28,6> K = (S.ldlt().solve(H*P)).transpose(); // P and S are symmetric
//Matrix<double,28,6> K = P*H.transpose()*S.inverse();//S.lu().solve(H*P).transpose();
Matrix<double,28,1> x_error = K*r;
// Apply Kalman gain
ApplyCorrection( x_error );
Matrix<double,28,28> T = Matrix<double,28,28>::Identity() - K*H;
P = T*P*T.transpose() + K*R*K.transpose();
// Symmetrize
P = (P + P.transpose())/2.0;
if (isKeyframe)
{
// Add new keyframe from current position/camera pose
UpdateKeyframe( );
// Add noise to lambda
P(15,15) += dLambda;
// Update keyframe reference, if given path is absolute
if (absolute)
{
p_kf_v = p_c_v;
q_kf_v.q = q_c_v;
}
}
}
