void bot_trans_print_trans(const BotTrans * tran)
{
double rpy[3];
bot_quat_to_roll_pitch_yaw(tran->rot_quat, rpy);
printf("t=(%f %f %f) rpy=(%f,%f,%f)", tran->trans_vec[0], tran->trans_vec[1],
tran->trans_vec[2], bot_to_degrees(rpy[0]), bot_to_degrees(rpy[1]), bot_to_degrees(rpy[2]));
}
void
bot_angle_axis_to_roll_pitch_yaw (double theta, const double axis[3],
double rpy[3])
{
double q[4];
bot_angle_axis_to_quat (theta, axis, q);
bot_quat_to_roll_pitch_yaw (q, rpy);
}
int
atrans_get_local_pos_heading(ATrans *atrans, double pos[3], double *heading)
{
BotTrans bt;
if (!atrans_get_trans(atrans, "body", "local", &bt))
return 0;
// Get position.
memcpy(pos, bt.trans_vec, 3 * sizeof(double));
// Get heading.
double rpy[3];
bot_quat_to_roll_pitch_yaw(bt.rot_quat, rpy);
*heading = rpy[2];
return 1;
}
void TvlqrControl::InitializeState(const mav_pose_t *msg) {
initial_state_ = PoseMsgToStateEstimatorVector(msg);
// get the yaw from the initial state
double rpy[3];
bot_quat_to_roll_pitch_yaw(msg->orientation, rpy);
Mz_ = rotz(-rpy[2]);
t0_ = GetTimestampNow();
state_initialized_ = true;
}
static void frames_update_handler(BotFrames *bot_frames, const char *frame, const char * relative_to, int64_t utime,
void *user)
{
RendererFrames *self = (RendererFrames *) user;
BotTrans tran;
int activeSensorNum = bot_gtk_param_widget_get_enum(self->pw, PARAM_FRAME_SELECT);
if (activeSensorNum > 0) {
const char * activeSensorName = self->frameNames[activeSensorNum];
if (strcmp(frame, activeSensorName) == 0) {
const char * relative_to = bot_frames_get_relative_to(self->frames, activeSensorName);
bot_frames_get_trans(self->frames, activeSensorName, relative_to, &tran);
double rpy[3];
bot_quat_to_roll_pitch_yaw(tran.rot_quat, rpy);
bot_gtk_param_widget_set_double(self->pw, PARAM_X, tran.trans_vec[0]);
bot_gtk_param_widget_set_double(self->pw, PARAM_Y, tran.trans_vec[1]);
bot_gtk_param_widget_set_double(self->pw, PARAM_Z, tran.trans_vec[2]);
bot_gtk_param_widget_set_double(self->pw, PARAM_ROLL, bot_to_degrees(rpy[0]));
bot_gtk_param_widget_set_double(self->pw, PARAM_PITCH, bot_to_degrees(rpy[1]));
bot_gtk_param_widget_set_double(self->pw, PARAM_YAW, bot_to_degrees(rpy[2]));
self->updating = 0;
}
}
bot_viewer_request_redraw(self->viewer);
}
int
bot_quaternion_test()
{
#define FAIL_TEST { fprintf(stderr, "bot_quaternion_test failed at line %d\n", \
__LINE__); return 0; }
fprintf(stderr, "running quaternion test\n");
double theta = 0;
double rvec[] = { 0, 0, 1 };
double q[4];
double rpy[3];
double roll, pitch, yaw;
bot_angle_axis_to_quat(theta, rvec, q);
if (! qeq (q, 1, 0, 0, 0)) FAIL_TEST;
bot_quat_to_roll_pitch_yaw (q, rpy);
roll = rpy[0]; pitch = rpy[1]; yaw = rpy[2];
if (! rpyeq (roll,pitch,yaw, 0,0,0)) FAIL_TEST;
// quat_from_angle_axis
theta = M_PI;
bot_angle_axis_to_quat(theta, rvec, q);
fprintf(stderr,"<%.3f, %.3f, %.3f, %.3f>\n", q[0], q[1], q[2], q[3]);
if (! qeq (q, 0, 0, 0, 1)) FAIL_TEST;
// bot_quat_to_angle_axis
bot_quat_to_angle_axis (q, &theta, rvec);
if (!feq (theta, M_PI)) FAIL_TEST;
if (!feq(rvec[0], 0) || !feq(rvec[1], 0) || !feq(rvec[2], 1)) FAIL_TEST;
bot_quat_to_roll_pitch_yaw (q, rpy);
if (! rpyeq (roll,pitch,yaw, 0,0,M_PI)) FAIL_TEST;
double q2[4];
double q3[4];
double axis1[] = { 0, 1, 0 };
double axis2[] = { 0, 0, 1 };
bot_angle_axis_to_quat (M_PI/2, axis1, q);
bot_angle_axis_to_quat (M_PI/2, axis2, q);
bot_quat_mult (q3, q, q2);
rvec[0] = 0; rvec[1] = 0; rvec[2] = 1;
bot_quat_rotate (q, rvec);
fprintf(stderr, "by q: [ %.2f, %.2f, %.2f ]\n", rvec[0], rvec[1], rvec[2]);
rvec[0] = 0; rvec[1] = 0; rvec[2] = 1;
bot_quat_rotate (q2, rvec);
fprintf(stderr, "by q2: [ %.2f, %.2f, %.2f ]\n", rvec[0], rvec[1], rvec[2]);
rvec[0] = 0; rvec[1] = 0; rvec[2] = 1;
bot_quat_rotate (q3, rvec);
fprintf(stderr, "by q*q2: [ %.2f, %.2f, %.2f ]\n",
rvec[0], rvec[1], rvec[2]);
rvec[0] = 0; rvec[1] = 0; rvec[2] = 1;
bot_quat_mult (q3, q2, q);
bot_quat_rotate (q3, rvec);
fprintf(stderr, "by q2*q: [ %.2f, %.2f, %.2f ]\n",
rvec[0], rvec[1], rvec[2]);
// TODO
#undef FAIL_TEST
fprintf(stderr, "bot_quaternion_test complete\n");
return 1;
}
static void
car_draw (Viewer *viewer, Renderer *super)
{
RendererCar *self = (RendererCar*) super->user;
botlcm_pose_t pose;
if (!atrans_get_local_pose (self->atrans, &pose))
return;
BotGtkParamWidget *pw = self->pw;
int bling = self->chassis_model ?
bot_gtk_param_widget_get_bool (pw, PARAM_NAME_BLING) : 0;
int wheels = self->wheel_model ?
bot_gtk_param_widget_get_bool (pw, PARAM_NAME_WHEELS) : 0;
if ((bling || wheels) && !self->display_lists_ready) {
load_bling (self);
}
glColor4f(0,1,0,0.75);
glLineWidth (2);
glBegin(GL_LINE_STRIP);
glVertex3dv (self->last_pose.pos);
for (unsigned int i = 0;
i < MIN (bot_ptr_circular_size(self->path), self->max_draw_poses);
i++) {
glVertex3dv(bot_ptr_circular_index(self->path, i));
}
glEnd();
glPushMatrix();
// compute the rotation matrix to orient the vehicle in world
// coordinates
double body_quat_m[16];
bot_quat_pos_to_matrix(pose.orientation, pose.pos, body_quat_m);
// opengl expects column-major matrices
double body_quat_m_opengl[16];
bot_matrix_transpose_4x4d (body_quat_m, body_quat_m_opengl);
// rotate and translate the vehicle
glMultMatrixd (body_quat_m_opengl);
glEnable (GL_DEPTH_TEST);
if (bling && self->display_lists_ready && self->chassis_dl)
draw_chassis_model (self);
else
draw_footprint (self);
if (wheels && self->display_lists_ready && self->wheel_dl)
draw_wheels (self);
glPopMatrix();
if (self->display_detail) {
char buf[256];
switch (self->display_detail)
{
case DETAIL_SPEED:
sprintf(buf, "%.2f m/s",
sqrt(sq(pose.vel[0]) + sq(pose.vel[1]) + sq(pose.vel[2])));
break;
case DETAIL_RPY:
{
double rpy[3];
bot_quat_to_roll_pitch_yaw(pose.orientation, rpy);
sprintf(buf, "r: %6.2f\np: %6.2f\ny: %6.2f", to_degrees(rpy[0]), to_degrees(rpy[1]), to_degrees(rpy[2]));
break;
}
}
glColor3f(1,1,1);
bot_gl_draw_text(pose.pos, GLUT_BITMAP_HELVETICA_12, buf,
BOT_GL_DRAW_TEXT_DROP_SHADOW);
}
}
RBISUpdateInterface * LegOdoCommon::createMeasurement(BotTrans &odo_positionT, BotTrans &odo_deltaT,
int64_t utime, int64_t prev_utime,
int odo_position_status, float odo_delta_status){
BotTrans odo_velT = getTransAsVelocityTrans(odo_deltaT, utime, prev_utime);
Eigen::MatrixXd cov_legodo_use;
LegOdoCommonMode mode_current = mode_;
if ( (mode_current == MODE_POSITION_AND_LIN_RATE) && (!odo_position_status) ){
if (verbose) std::cout << "LegOdo Mode is MODE_POSITION_AND_LIN_RATE but position is not suitable\n";
if (verbose) std::cout << "Falling back to lin rate only for this iteration\n";
mode_current = MODE_LIN_RATE;
}
bool delta_certain = true;
if (odo_delta_status < 0.5){ // low variance, high reliable
delta_certain = true;
}else{ // high variance, low reliable e.g. breaking contact
delta_certain = false;
}
Eigen::MatrixXd cov_legodo;
Eigen::VectorXi z_indices;
getCovariance(mode_current, delta_certain, cov_legodo, z_indices );
if (mode_current == MODE_LIN_AND_ROT_RATE) {
// Apply a linear and rotation rate correction
// This was finished in Feb 2015 but not tested on the robot
Eigen::VectorXd z_meas(6);
double rpy[3];
bot_quat_to_roll_pitch_yaw(odo_deltaT.rot_quat,rpy);
double elapsed_time = ( (double) utime - prev_utime)/1000000;
double rpy_rate[3];
rpy_rate[0] = rpy[0]/elapsed_time;
rpy_rate[1] = rpy[1]/elapsed_time;
rpy_rate[2] = rpy[2]/elapsed_time;
z_meas.head<3>() = Eigen::Map<const Eigen::Vector3d>(odo_velT.trans_vec);
z_meas.tail<3>() = Eigen::Map<const Eigen::Vector3d>(rpy_rate);
return new RBISIndexedMeasurement(z_indices, z_meas, cov_legodo, RBISUpdateInterface::legodo,
utime);
}else if (mode_current == MODE_LIN_RATE) {
return new RBISIndexedMeasurement(z_indices,
Eigen::Map<const Eigen::Vector3d>( odo_velT.trans_vec ), cov_legodo, RBISUpdateInterface::legodo,
utime);
}else if (mode_current == MODE_POSITION_AND_LIN_RATE) {
// Newly added mode
if (verbose) std::cout << "LegOdometry Mode update both xyz position and rate\n";
Eigen::VectorXd z_meas(6);
z_meas.head<3>() = Eigen::Map<const Eigen::Vector3d>(odo_positionT.trans_vec);
z_meas.tail<3>() = Eigen::Map<const Eigen::Vector3d>(odo_velT.trans_vec);
return new RBISIndexedMeasurement(z_indices, z_meas, cov_legodo, RBISUpdateInterface::legodo,
utime);
}else{
std::cout << "LegOdometry Mode not supported, exiting\n";
return NULL;
}
}
