void
rotate_camera (carmen_orientation_3D_t rotation)
{
rotation_matrix* cam_matrix = create_rotation_matrix (camera_pose.orientation);
rotation_matrix* r_matrix = create_rotation_matrix (rotation);
rotation_matrix* new_cam_matrix = multiply_matrix_matrix (r_matrix, cam_matrix);
camera_pose.orientation = get_angles_from_rotation_matrix (new_cam_matrix);
destroy_rotation_matrix (cam_matrix);
destroy_rotation_matrix (r_matrix);
destroy_rotation_matrix (new_cam_matrix);
}
static carmen_vector_3D_t
get_xsens_position_global_reference (carmen_pose_3D_t xsens_pose, carmen_pose_3D_t sensor_board_pose, carmen_pose_3D_t car_pose)
{
rotation_matrix* board_to_car_matrix = create_rotation_matrix (sensor_board_pose.orientation);
rotation_matrix* car_to_global_matrix = create_rotation_matrix (car_pose.orientation);
carmen_vector_3D_t car_reference = multiply_matrix_vector (board_to_car_matrix, xsens_pose.position);
car_reference = add_vectors (car_reference, sensor_board_pose.position);
carmen_vector_3D_t global_reference = multiply_matrix_vector (car_to_global_matrix, car_reference);
global_reference = add_vectors (global_reference, car_pose.position);
destroy_rotation_matrix (board_to_car_matrix);
destroy_rotation_matrix (car_to_global_matrix);
return global_reference;
}
ErrorCode rotate_coords(FloatValue *coords, FloatValue rotangle, IndexValue bond, struct ResidueData *residues, IndexValue res)
{
IndexValue atom1, atom2;
FloatValue x1, y1, z1;
FloatValue x2, y2, z2;
FloatValue rx, ry, rz;
FloatValue matxx, matxy, matxz;
FloatValue matyx, matyy, matyz;
FloatValue matzx, matzy, matzz;
IndexValue lastatom;
FloatValue dx, dy, dz;
FloatValue x, y, z;
IndexValue atom;
atom1 = residues[res].bonds[bond].atom1;
x1 = coords[xcoord_index(atom1)];
y1 = coords[ycoord_index(atom1)];
z1 = coords[zcoord_index(atom1)];
atom2 = residues[res].bonds[bond].atom2;
x2 = coords[xcoord_index(atom2)];
y2 = coords[ycoord_index(atom2)];
z2 = coords[zcoord_index(atom2)];
// This is the index of the last atom in the array affected by this rotation,.
lastatom = residues[res].bonds[bond].lastatom;
// Calculating normalized rotation axis.
vector_subtract(&rx, &ry, &rz, x2, y2, z2, x1, y1, z1);
vector_normalize(&rx, &ry, &rz);
// Calculating rotation matrix for rotation angle rotangle around rotation axis (rx, ry, rz).
create_rotation_matrix(&matxx, &matxy, &matxz, &matyx, &matyy, &matyz, &matzx, &matzy, &matzz, rotangle, rx, ry, rz);
// for (atom = atom2 + 1; atom < lastatom; atom++)
for (atom = atom2 + 1; atom <= lastatom; atom++)
{
dx = coords[xcoord_index(atom)] - x2;
dy = coords[ycoord_index(atom)] - y2;
dz = coords[zcoord_index(atom)] - z2;
x = matxx * dx + matxy * dy + matxz * dz;
y = matyx * dx + matyy * dy + matyz * dz;
z = matzx * dx + matzy * dy + matzz * dz;
coords[xcoord_index(atom)] = x + x2;
coords[ycoord_index(atom)] = y + y2;
coords[zcoord_index(atom)] = z + z2;
}
return NO_ERROR;
}
void
move_camera (carmen_vector_3D_t displacement)
{
rotation_matrix* r_matrix = create_rotation_matrix (camera_pose.orientation);
carmen_vector_3D_t displacement_world_coordinates = multiply_matrix_vector (r_matrix, displacement);
camera_pose.position = add_vectors (camera_pose.position, displacement_world_coordinates);
destroy_rotation_matrix (r_matrix);
}
void add_velodyne_message(velodyne_360_drawer* v_drawer, carmen_velodyne_partial_scan_message* velodyne_message)
{
static double vertical_correction[32] = { -30.67, -9.3299999, -29.33, -8.0, -28.0, -6.6700001, -26.67, -5.3299999, -25.33, -4.0,
-24.0, -2.6700001, -22.67, -1.33, -21.33, 0.0, -20.0, 1.33, -18.67, 2.6700001, -17.33, 4.0, -16.0, 5.3299999, -14.67, 6.6700001,
-13.33, 8.0, -12.0, 9.3299999, -10.67, 10.67 };
switch_current_last(v_drawer);
int num_points = velodyne_message->number_of_32_laser_shots*32;
v_drawer->num_points_current = num_points;
v_drawer->points_current = realloc(v_drawer->points_current, num_points*sizeof(carmen_vector_3D_t));
v_drawer->angles_current = realloc(v_drawer->angles_current, num_points*sizeof(double));
rotation_matrix* velodyne_to_board_matrix = create_rotation_matrix(v_drawer->velodyne_pose.orientation);
rotation_matrix* board_to_car_matrix = create_rotation_matrix(v_drawer->sensor_board_pose.orientation);
int i;
for(i = 0; i < velodyne_message->number_of_32_laser_shots; i++)
{
double rot_angle = carmen_degrees_to_radians(velodyne_message->partial_scan[i].angle);
int j;
for(j = 0; j < 32; j++)
{
double vert_angle = carmen_degrees_to_radians(vertical_correction[j]);
carmen_vector_3D_t point_position = get_velodyne_point_car_reference( -rot_angle, vert_angle,
velodyne_message->partial_scan[i].distance[j]/500.0,
v_drawer->velodyne_pose, v_drawer->sensor_board_pose,
velodyne_to_board_matrix, board_to_car_matrix);
v_drawer->points_current[i*32 + j] = point_position;
v_drawer->angles_current[i*32 + j] = rot_angle;
}
}
destroy_rotation_matrix(velodyne_to_board_matrix);
destroy_rotation_matrix(board_to_car_matrix);
}
void
rotate_coords_about_axis_dp_list(coord_3D **coords, int num_coords, coord_3D point,
vectormag_3D direction, double theta)
{
int i;
double *matrix;
matrix = create_rotation_matrix(point, direction, theta);
for(i = 0; i < num_coords; i++)
apply_rotation_matrix_to_coord_p(coords[i], matrix);
destroy_rotation_matrix(matrix);
}
static int
read_parameters(int argc, char **argv)
{
int num_items;
carmen_param_t param_list[] =
{
{(char*)"sensor_board_1", (char*)"x", CARMEN_PARAM_DOUBLE, &(sensor_board_1_pose.position.x), 0, NULL},
{(char*)"sensor_board_1", (char*)"y", CARMEN_PARAM_DOUBLE, &(sensor_board_1_pose.position.y), 0, NULL},
{(char*)"sensor_board_1", (char*)"z", CARMEN_PARAM_DOUBLE, &(sensor_board_1_pose.position.z), 0, NULL},
{(char*)"sensor_board_1", (char*)"roll", CARMEN_PARAM_DOUBLE, &(sensor_board_1_pose.orientation.roll),0, NULL},
{(char*)"sensor_board_1", (char*)"pitch", CARMEN_PARAM_DOUBLE, &(sensor_board_1_pose.orientation.pitch),0, NULL},
{(char*)"sensor_board_1", (char*)"yaw", CARMEN_PARAM_DOUBLE, &(sensor_board_1_pose.orientation.yaw), 0, NULL},
{(char*) "velodyne", (char*) "x", CARMEN_PARAM_DOUBLE, &(velodyne_pose.position.x), 0, NULL},
{(char*) "velodyne", (char*) "y", CARMEN_PARAM_DOUBLE, &(velodyne_pose.position.y), 0, NULL},
{(char*) "velodyne", (char*) "z", CARMEN_PARAM_DOUBLE, &(velodyne_pose.position.z), 0, NULL},
{(char*) "velodyne", (char*) "roll", CARMEN_PARAM_DOUBLE, &(velodyne_pose.orientation.roll), 0, NULL},
{(char*) "velodyne", (char*) "pitch", CARMEN_PARAM_DOUBLE, &(velodyne_pose.orientation.pitch), 0, NULL},
{(char*) "velodyne", (char*) "yaw", CARMEN_PARAM_DOUBLE, &(velodyne_pose.orientation.yaw), 0, NULL},
{(char *)"robot", (char*)"length", CARMEN_PARAM_DOUBLE, &car_config.length, 0, NULL},
{(char *)"robot", (char*)"width", CARMEN_PARAM_DOUBLE, &car_config.width, 0, NULL},
{(char *)"robot", (char*)"distance_between_rear_car_and_rear_wheels", CARMEN_PARAM_DOUBLE, &car_config.distance_between_rear_car_and_rear_wheels, 1, NULL},
{(char *)"robot", (char*)"distance_between_front_and_rear_axles", CARMEN_PARAM_DOUBLE, &car_config.distance_between_front_and_rear_axles, 1, NULL},
{(char *)"robot", (char*)"wheel_radius", CARMEN_PARAM_DOUBLE, &robot_wheel_radius, 0, NULL},
{(char*)"moving_objects", (char*)"safe_range_above_sensors", CARMEN_PARAM_DOUBLE, &safe_range_above_sensors, 0, NULL},
{(char*)"localize_ackerman", (char*)"number_of_sensors", CARMEN_PARAM_INT, &number_of_sensors, 0, NULL},
};
num_items = sizeof(param_list)/sizeof(param_list[0]);
carmen_param_install_params(argc, argv, param_list, num_items);
sensor_board_1_to_car_matrix = create_rotation_matrix(sensor_board_1_pose.orientation);
get_alive_sensors(argc, argv);
get_sensors_param(argc, argv);
return 0;
}
static void display_func(void) {
static GLfloat v[4][4] = {
{ 1.0, 0.0, 0.0, 0.0 },
{ 0.0, 1.0, 0.0, 0.0 },
{ 0.0, 0.0, -1.0, 0.0 },
{ 0.0, 0.0, 0.0, 1.0 } };
GLfloat m[4][4];
int i, j;
glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
glLoadIdentity();
glMultMatrixf(&v[0][0]);
glPushMatrix();
create_rotation_matrix(m, &curr);
glScalef(scale_factor, scale_factor, scale_factor);
glMultMatrixf(&m[0][0]);
draw();
glPopMatrix();
glutSwapBuffers();
}
static void
get_sensors_param(int argc, char **argv)
{
int i, j;
int flipped;
int horizontal_resolution;
char stereo_velodyne_string[256];
int stereo_velodyne_vertical_roi_ini;
int stereo_velodyne_vertical_roi_end;
int stereo_velodyne_horizontal_roi_ini;
int stereo_velodyne_horizontal_roi_end;
int roi_ini, roi_end;
spherical_sensor_params[0].pose = velodyne_pose;
spherical_sensor_params[0].sensor_robot_reference = carmen_change_sensor_reference(sensor_board_1_pose.position, spherical_sensor_params[0].pose.position, sensor_board_1_to_car_matrix);
spherical_sensor_params[0].height = spherical_sensor_params[0].sensor_robot_reference.z + robot_wheel_radius;
if (spherical_sensor_params[0].height > highest_sensor)
highest_sensor = spherical_sensor_params[0].height;
if (spherical_sensor_params[0].alive && !strcmp(spherical_sensor_params[0].name,"velodyne"))
{
spherical_sensor_params[0].ray_order = carmen_velodyne_get_ray_order();
spherical_sensor_params[0].vertical_correction = carmen_velodyne_get_vertical_correction();
spherical_sensor_params[0].delta_difference_mean = carmen_velodyne_get_delta_difference_mean();
spherical_sensor_params[0].delta_difference_stddev = carmen_velodyne_get_delta_difference_stddev();
carmen_param_t param_list[] =
{
{spherical_sensor_params[0].name, (char*)"vertical_resolution", CARMEN_PARAM_INT, &spherical_sensor_params[0].vertical_resolution, 0, NULL},
{(char *)"localize_ackerman", (char*)"velodyne_range_max", CARMEN_PARAM_DOUBLE, &spherical_sensor_params[0].range_max, 0, NULL},
{spherical_sensor_params[0].name, (char*)"time_spent_by_each_scan", CARMEN_PARAM_DOUBLE, &spherical_sensor_params[0].time_spent_by_each_scan, 0, NULL},
};
carmen_param_install_params(argc, argv, param_list, sizeof(param_list) / sizeof(param_list[0]));
init_velodyne_points(&spherical_sensor_data[0].points, &spherical_sensor_data[0].intensity, &spherical_sensor_data[0].robot_pose,
&spherical_sensor_data[0].robot_velocity, &spherical_sensor_data[0].robot_timestamp, &spherical_sensor_data[0].robot_phi);
spherical_sensor_params[0].sensor_to_support_matrix = create_rotation_matrix(spherical_sensor_params[0].pose.orientation);
spherical_sensor_data[0].point_cloud_index = 0;
carmen_prob_models_alloc_sensor_data(&spherical_sensor_data[0], spherical_sensor_params[0].vertical_resolution, number_of_threads);
if (max_range < spherical_sensor_params[0].range_max)
{
max_range = spherical_sensor_params[0].range_max;
}
spherical_sensor_params[0].current_range_max = spherical_sensor_params[0].range_max;
}
for (i = 1; i < number_of_sensors; i++)
{
if (spherical_sensor_params[i].alive)
{
spherical_sensor_params[i].pose = get_stereo_velodyne_pose_3D(argc, argv, i);
spherical_sensor_params[i].sensor_robot_reference = carmen_change_sensor_reference(sensor_board_1_pose.position, spherical_sensor_params[i].pose.position, sensor_board_1_to_car_matrix);
spherical_sensor_params[i].height = spherical_sensor_params[i].sensor_robot_reference.z + robot_wheel_radius;
if (spherical_sensor_params[i].height > highest_sensor)
highest_sensor = spherical_sensor_params[i].height;
sprintf(stereo_velodyne_string, "%s%d", "stereo", i);
carmen_param_t param_list[] =
{
{spherical_sensor_params[i].name, (char*) "vertical_resolution", CARMEN_PARAM_INT, &spherical_sensor_params[i].vertical_resolution, 0, NULL},
{spherical_sensor_params[i].name, (char*) "horizontal_resolution", CARMEN_PARAM_INT, &horizontal_resolution, 0, NULL},
{spherical_sensor_params[i].name, (char*) "flipped", CARMEN_PARAM_ONOFF, &flipped, 0, NULL},
{spherical_sensor_params[i].name, (char*) "range_max", CARMEN_PARAM_DOUBLE, &spherical_sensor_params[i].range_max, 0, NULL},
{spherical_sensor_params[i].name, (char*) "vertical_roi_ini", CARMEN_PARAM_INT, &stereo_velodyne_vertical_roi_ini, 0, NULL },
{spherical_sensor_params[i].name, (char*) "vertical_roi_end", CARMEN_PARAM_INT, &stereo_velodyne_vertical_roi_end, 0, NULL },
{spherical_sensor_params[i].name, (char*) "horizontal_roi_ini", CARMEN_PARAM_INT, &stereo_velodyne_horizontal_roi_ini, 0, NULL },
{spherical_sensor_params[i].name, (char*) "horizontal_roi_end", CARMEN_PARAM_INT, &stereo_velodyne_horizontal_roi_end, 0, NULL }
};
carmen_param_install_params(argc, argv, param_list, sizeof(param_list) / sizeof(param_list[0]));
if (flipped)
{
spherical_sensor_params[i].vertical_resolution = horizontal_resolution;
roi_ini = stereo_velodyne_horizontal_roi_ini;
roi_end = stereo_velodyne_horizontal_roi_end;
}
else
{
roi_ini = stereo_velodyne_vertical_roi_ini;
roi_end = stereo_velodyne_vertical_roi_end;
}
if (spherical_sensor_params[i].vertical_resolution > (roi_end - roi_ini))
{
carmen_die("The stereo_velodyne_vertical_resolution is bigger than stereo point cloud height");
}
if (max_range < spherical_sensor_params[i].range_max)
{
max_range = spherical_sensor_params[i].range_max;
}
spherical_sensor_params[i].current_range_max = spherical_sensor_params[i].range_max;
spherical_sensor_params[i].range_max_factor = 1.0;
spherical_sensor_params[i].ray_order = generates_ray_order(spherical_sensor_params[i].vertical_resolution);
spherical_sensor_params[i].vertical_correction = get_variable_velodyne_correction();
// spherical_sensor_params[i].vertical_correction = get_stereo_velodyne_correction(flipped, i, spherical_sensor_params[i].vertical_resolution, roi_ini, roi_end, 0, 0);
init_velodyne_points(&spherical_sensor_data[i].points, &spherical_sensor_data[i].intensity, &spherical_sensor_data[i].robot_pose,
&spherical_sensor_data[i].robot_velocity, &spherical_sensor_data[i].robot_timestamp, &spherical_sensor_data[i].robot_phi);
spherical_sensor_params[i].sensor_to_support_matrix = create_rotation_matrix(spherical_sensor_params[i].pose.orientation);
spherical_sensor_data[i].point_cloud_index = 0;
carmen_prob_models_alloc_sensor_data(&spherical_sensor_data[i], spherical_sensor_params[i].vertical_resolution, number_of_threads);
//TODO : tem que fazer esta medida para as cameras igual foi feito para o velodyne
if(i == 8) {
spherical_sensor_params[i].delta_difference_mean = carmen_velodyne_get_delta_difference_mean();
spherical_sensor_params[i].delta_difference_stddev = carmen_velodyne_get_delta_difference_stddev();
} else {
spherical_sensor_params[i].delta_difference_mean = (double *)calloc(50, sizeof(double));
spherical_sensor_params[i].delta_difference_stddev = (double *)calloc(50, sizeof(double));
for (j = 0; j < 50; j++)
spherical_sensor_params[i].delta_difference_stddev[j] = 1.0;
}
}
}
}
void RPYSolver::orientationSolver(double* output, double phi, double theta, double psi, double yaw1, double pitch1, double roll1, double yaw2, double pitch2, double roll2, int attempt)
{
/**************************************************************************/
//Is the desired orientation possible to attain with the given joint limits?
/**************************************************************************/
double wrist_pitch_limit_max; //Used to express the wrist pitch limit in radians
double wrist_pitch_limit_min; //Used to express the wrist pitch limit in radians
//The wrist pitch limit in radians
wrist_pitch_limit_max=(PI/180.0)*WRIST_PITCH_LIMIT_MAX;
wrist_pitch_limit_min=(PI/180.0)*WRIST_PITCH_LIMIT_MIN;
//The PR2 FK is defined such that pitch is negative upward. Hence, the input pitch values are negated, because the following portion of the code was written assuming that pitch is positive upward.
theta=-theta;
pitch1=-pitch1;
pitch2=-pitch2;
double v1[] = {cos(theta)*cos(phi), cos(theta)*sin(phi), sin(theta)};
double v2[] = {cos(pitch2)*cos(yaw2), cos(pitch2)*sin(yaw2), sin(pitch2)};
double dp_v = dot_product(v1, v2, 3);
double check_flag=dp_v<0;
double check_flag2=fabs(dp_v)>fabs(cos(wrist_pitch_limit_max));
double check_flag3=dp_v>0;
double check_flag4=dp_v>cos(wrist_pitch_limit_min);
if((check_flag && check_flag2) || (check_flag3 && check_flag4)) {
// std::cout << "Impossible." << std::endl;
output[0]=0;
output[1]=0;
output[2]=0;
output[3]=0;
return;
}
/*****************************************************/
//Firstly, all variables are declared and some defined
/*****************************************************/
double hand1[]={0, -0.5, 0, 0, 0.5, 0};
double hand2[]={0, -0.5, 0, 0, 0.5, 0};
double hand1_fo[6];
double hand2_fo[6];
double hand1_vect[3], hand2_vect[3];
double grip1[]={0, 0, 0, 1, 0, 0};
double grip2[]={0, 0, 0, 1, 0, 0};
double grip1_fo[6];
double grip2_fo[6];
double grip1_vect[3], grip2_vect[3];
double indicator1[]={-2.5, 0, 0, -2.5, 0, 1};
double indicator2[]={-2.5, 0, 0, -2.5, 0, 1};
double indicator1_fo[6];
double indicator2_fo[6];
double ind1_vect[3],ind2_vect[3];
double comp1_vect[3], comp2_vect[3], project1_vect[3], project2_vect[3];
double rot1[9], rot2[9], rot3[9];
double temp[6], temp1[9], temp2[9], temp3[9], temp_var, temp_vect[3], temp2_vect[3];
double w[3], w_hat[9];
double rot_world[9], rot_world_trans[9];
double identity[]={1, 0, 0,
0, 1, 0,
0, 0, 1};
double unit_x[3]={1, 0, 0};
double zero_vect[3]={0, 0, 0};
double fo_arm_roll, wrist_pitch, wrist_roll, fs_angle, fe_angle;
double is_orient_possible_flag=1; //Flag returned indicates whether desired orientation is possible
//with wrist limits. 1-possible, 0-impossible
double c_alpha, c_beta, c_gamma, c_delta, c_eps;
/******************************/
//Accepting the input arguments
/******************************/
create_rotation_matrix(rot_world,phi,theta,psi);
transpose(rot_world_trans, rot_world, 3, 3);
//The start and the end orientations in the world frame
rot1[0]=cos(yaw1); rot1[3]=-sin(yaw1); rot1[6]=0;
rot1[1]=sin(yaw1); rot1[4]=cos(yaw1); rot1[7]=0;
rot1[2]=0; rot1[5]=0; rot1[8]=1;
rot2[0]=cos(pitch1); rot2[3]=0; rot2[6]=-sin(pitch1);
rot2[1]=0; rot2[4]=1; rot2[7]=0;
rot2[2]=sin(pitch1); rot2[5]=0; rot2[8]=cos(pitch1);
multiply(rot3,rot1,3,3,rot2,3); //Yaw and pitch rotations
multiply(temp,rot3,3,3,hand1,2);
equate(hand1,temp,3,2);
multiply(temp,rot3,3,3,grip1,2);
equate(grip1,temp,3,2);
w[0]=grip1[3]; //Unit vector along grip
w[1]=grip1[4];
w[2]=grip1[5];
w_hat[0]=0; w_hat[3]=-w[2]; w_hat[6]=w[1];
w_hat[1]=w[2]; w_hat[4]=0; w_hat[7]=-w[0];
w_hat[2]=-w[1]; w_hat[5]=w[0]; w_hat[8]=0;
scalar_multiply(temp1,w_hat,3,3,sin(roll1));
multiply(temp2,w_hat,3,3,w_hat,3);
scalar_multiply(temp3,temp2,3,3,1-cos(roll1));
matrix_add(temp2,temp1,temp3,3,3);
matrix_add(rot1,identity, temp2,3,3);
multiply(temp1,rot1,3,3,hand1,2);
equate(hand1,temp1,3,2);
rot1[0]=cos(yaw2); rot1[3]=-sin(yaw2); rot1[6]=0;
rot1[1]=sin(yaw2); rot1[4]=cos(yaw2); rot1[7]=0;
rot1[2]=0; rot1[5]=0; rot1[8]=1;
rot2[0]=cos(pitch2); rot2[3]=0; rot2[6]=-sin(pitch2);
rot2[1]=0; rot2[4]=1; rot2[7]=0;
rot2[2]=sin(pitch2); rot2[5]=0; rot2[8]=cos(pitch2);
multiply(rot3,rot1,3,3,rot2,3); //Yaw and pitch rotations
multiply(temp,rot3,3,3,hand2,2);
equate(hand2,temp,3,2);
multiply(temp,rot3,3,3,grip2,2);
equate(grip2,temp,3,2);
w[0]=grip2[3]; //Unit vector along grip
w[1]=grip2[4];
w[2]=grip2[5];
w_hat[0]=0; w_hat[3]=-w[2]; w_hat[6]=w[1];
w_hat[1]=w[2]; w_hat[4]=0; w_hat[7]=-w[0];
w_hat[2]=-w[1]; w_hat[5]=w[0]; w_hat[8]=0;
scalar_multiply(temp1,w_hat,3,3,sin(roll2));
multiply(temp2,w_hat,3,3,w_hat,3);
scalar_multiply(temp3,temp2,3,3,1-cos(roll2));
matrix_add(temp2,temp1,temp3,3,3);
matrix_add(rot1,identity,temp2,3,3);
multiply(temp1,rot1,3,3,hand2,2);
equate(hand2,temp1,3,2);
//The start and the end orientations in the forearm frame
multiply(hand1_fo,rot_world_trans,3,3,hand1,2);
multiply(hand2_fo,rot_world_trans,3,3,hand2,2);
multiply(grip1_fo,rot_world_trans,3,3,grip1,2);
multiply(grip2_fo,rot_world_trans,3,3,grip2,2);
grip1_vect[0]=grip1_fo[3];
grip1_vect[1]=grip1_fo[4];
grip1_vect[2]=grip1_fo[5];
temp_var=dot_product(grip1_vect, unit_x, 3);
scalar_multiply(comp1_vect, unit_x,3,1,temp_var);
subtract(project1_vect,grip1_vect,comp1_vect,3,1);
grip2_vect[0]=grip2_fo[3];
grip2_vect[1]=grip2_fo[4];
grip2_vect[2]=grip2_fo[5];
temp_var=dot_product(grip2_vect, unit_x, 3);
scalar_multiply(comp2_vect, unit_x,3,1,temp_var);
subtract(project2_vect,grip2_vect,comp2_vect,3,1);
ind1_vect[0]=indicator1[3]-indicator1[0];
ind1_vect[1]=indicator1[4]-indicator1[1];
ind1_vect[2]=indicator1[5]-indicator1[2];
ind2_vect[0]=indicator2[3]-indicator2[0];
ind2_vect[1]=indicator2[4]-indicator2[1];
ind2_vect[2]=indicator2[5]-indicator2[2];
if(!check_equality(grip1_vect, comp1_vect, 3)) {
c_delta=dot_product(ind1_vect, project1_vect, 3)/vect_norm(project1_vect,3);
fs_angle=acos(c_delta);
cross_product(temp_vect, ind1_vect, project1_vect);
if(vect_divide(temp_vect, unit_x, 3)<0) {
fs_angle=-fs_angle;
}
rot1[0]=1; rot1[3]=0; rot1[6]=0;
rot1[1]=0; rot1[4]=cos(fs_angle); rot1[7]=-sin(fs_angle);
rot1[2]=0; rot1[5]=sin(fs_angle); rot1[8]=cos(fs_angle);
multiply(temp, rot1, 3,3, indicator1, 2);
equate(indicator1_fo, temp, 3, 2);
}
else {
equate(indicator1_fo, indicator1, 3,2);
}
if(!check_equality(grip2_vect, comp2_vect, 3)) {
c_eps=dot_product(ind2_vect, project2_vect, 3)/vect_norm(project2_vect, 3);
fe_angle=acos(c_eps);
cross_product(temp_vect, ind2_vect, project2_vect);
if(vect_divide(temp_vect, unit_x, 3)<0) {
fe_angle=-fe_angle;
}
rot1[0]=1; rot1[3]=0; rot1[6]=0;
rot1[1]=0; rot1[4]=cos(fe_angle); rot1[7]=-sin(fe_angle);
rot1[2]=0; rot1[5]=sin(fe_angle); rot1[8]=cos(fe_angle);
multiply(temp, rot1, 3,3, indicator2, 2);
equate(indicator2_fo, temp, 3, 2);
}
else {
equate(indicator2_fo, indicator2, 3,2);
}
/*********************************/
//Forearm roll is calculated now
/*********************************/
ind1_vect[0]=indicator1_fo[3]-indicator1_fo[0];
ind1_vect[1]=indicator1_fo[4]-indicator1_fo[1];
ind1_vect[2]=indicator1_fo[5]-indicator1_fo[2];
ind2_vect[0]=indicator2_fo[3]-indicator2_fo[0];
ind2_vect[1]=indicator2_fo[4]-indicator2_fo[1];
ind2_vect[2]=indicator2_fo[5]-indicator2_fo[2];
c_gamma=dot_product(ind1_vect, ind2_vect, 3);
cross_product(temp_vect, ind1_vect, ind2_vect);
if(vect_divide(temp_vect, unit_x, 3)>0) {
fo_arm_roll=acos(c_gamma);
}
else {
fo_arm_roll=-acos(c_gamma);
}
//In the second attempt try rotating the other way around to the same orientation
if(attempt == 2) {fo_arm_roll = -(2*PI - fo_arm_roll);}
rot1[0]=1; rot1[3]=0; rot1[6]=0;
rot1[1]=0; rot1[4]=cos(fo_arm_roll); rot1[7]=-sin(fo_arm_roll);
rot1[2]=0; rot1[5]=sin(fo_arm_roll); rot1[8]=cos(fo_arm_roll);
multiply(temp1,rot1,3,3,hand1_fo,2);
equate(hand1_fo,temp1,3,2);
multiply(temp1,rot1,3,3,grip1_fo,2);
equate(grip1_fo,temp1,3,2);
/*********************************/
//Wrist pitch is calculated now
/*********************************/
grip1_vect[0]=grip1_fo[3];
grip1_vect[1]=grip1_fo[4];
grip1_vect[2]=grip1_fo[5];
grip2_vect[0]=grip2_fo[3];
grip2_vect[1]=grip2_fo[4];
grip2_vect[2]=grip2_fo[5];
cross_product(temp_vect, grip1_vect, grip2_vect);
if(!check_equality(temp_vect, zero_vect, 3)) {
c_alpha=dot_product(grip1_vect, grip2_vect, 3);
cross_product(temp2_vect,ind2_vect,temp_vect);
if(vect_divide(temp2_vect, unit_x, 3)>0) {
wrist_pitch=-acos(c_alpha);
temp_vect[0]=-temp_vect[0];
temp_vect[1]=-temp_vect[1];
temp_vect[2]=-temp_vect[2];
}
else {
wrist_pitch=acos(c_alpha);
}
scalar_multiply(w, temp_vect, 3,1,1/vect_norm(temp_vect, 3));
w_hat[0]=0; w_hat[3]=-w[2]; w_hat[6]=w[1];
w_hat[1]=w[2]; w_hat[4]=0; w_hat[7]=-w[0];
w_hat[2]=-w[1]; w_hat[5]=w[0]; w_hat[8]=0;
scalar_multiply(temp1,w_hat,3,3,sin(wrist_pitch));
multiply(temp2,w_hat,3,3,w_hat,3);
scalar_multiply(temp3,temp2,3,3,1-cos(wrist_pitch));
matrix_add(temp2,temp1,temp3,3,3);
matrix_add(rot1,identity, temp2,3,3);
multiply(temp1,rot1,3,3,hand1_fo,2);
equate(hand1_fo,temp1,3,2);
multiply(temp1,rot1,3,3,grip1_fo,2);
equate(grip1_fo,temp1,3,2);
}
else {
wrist_pitch=0;
}
/**Somehow the wrist_roll calculations from within this code don't seem to be right.
This problem has been temporarily solved by invoking FK from environment_robarm3d.cpp**/
/*********************************/
//Wrist roll is calculated now
/*********************************/
wrist_roll=0;
hand1_vect[0]=hand1_fo[3]-hand1_fo[0];
hand1_vect[1]=hand1_fo[4]-hand1_fo[1];
hand1_vect[2]=hand1_fo[5]-hand1_fo[2];
hand2_vect[0]=hand2_fo[3]-hand2_fo[0];
hand2_vect[1]=hand2_fo[4]-hand2_fo[1];
hand2_vect[2]=hand2_fo[5]-hand2_fo[2];
grip1_vect[0]=grip1_fo[3];
grip1_vect[1]=grip1_fo[4];
grip1_vect[2]=grip1_fo[5];
c_beta=dot_product(hand1_vect, hand2_vect, 3);
cross_product(temp_vect, hand1_vect, hand2_vect);
if(!check_equality(temp_vect, zero_vect, 3)) {
if(vect_divide(temp_vect, grip1_vect, 3)>0) {
wrist_roll=acos(c_beta);
}
else {
wrist_roll=-acos(c_beta);
}
}
//The output
output[0]=is_orient_possible_flag;
output[1]=fo_arm_roll;
output[2]=wrist_pitch;
output[3]=wrist_roll;
}
void
velodyne_intensity_drawer_add_velodyne_message(velodyne_intensity_drawer* v_drawer, carmen_velodyne_partial_scan_message* velodyne_message, carmen_pose_3D_t car_fused_pose, carmen_vector_3D_t car_fused_velocity, double car_fused_time)
{
static double vertical_correction[32] = { -30.67, -9.3299999, -29.33, -8.0, -28.0, -6.6700001, -26.67, -5.3299999, -25.33, -4.0,
-24.0, -2.6700001, -22.67, -1.33, -21.33, 0.0, -20.0, 1.33, -18.67, 2.6700001, -17.33, 4.0, -16.0, 5.3299999, -14.67, 6.6700001,
-13.33, 8.0, -12.0, 9.3299999, -10.67, 10.67 };
static point_cloud velodyne_points = {NULL, NULL, {0.0, 0.0, 0.0}, 0, 0.0};
static double last_timestamp = 0.0;
if (last_timestamp == 0.0)
{
last_timestamp = velodyne_message->timestamp;
return;
}
int num_points = velodyne_message->number_of_32_laser_shots*(32);
if(num_points > velodyne_points.num_points)
{
velodyne_points.points = realloc(velodyne_points.points, num_points*sizeof(carmen_vector_3D_t));
velodyne_points.point_color = realloc(velodyne_points.point_color, num_points*sizeof(carmen_vector_3D_t));
}
velodyne_points.num_points = num_points;
velodyne_points.car_position = car_fused_pose.position;
velodyne_points.timestamp = velodyne_message->timestamp;
rotation_matrix* velodyne_to_board_matrix = create_rotation_matrix(v_drawer->velodyne_pose.orientation);
rotation_matrix* board_to_car_matrix = create_rotation_matrix(v_drawer->sensor_board_pose.orientation);
rotation_matrix* r_matrix_car_to_global = create_rotation_matrix(car_fused_pose.orientation);
int k = 0;
int i;
for(i = 0; i < velodyne_message->number_of_32_laser_shots; i++)
{
double shot_angle = -carmen_degrees_to_radians(velodyne_message->partial_scan[i].angle);
if(check_angle_in_range(shot_angle, v_drawer->horizontal_angle, v_drawer->horizontal_range))
{
int j;
for(j = 0; j < 32; j++)
{
if( j == v_drawer->vertical_position )
{
carmen_vector_3D_t point_position = get_velodyne_point_car_reference( shot_angle,
carmen_degrees_to_radians(vertical_correction[j]),
velodyne_message->partial_scan[i].distance[j]/500.0,
velodyne_to_board_matrix, board_to_car_matrix,
v_drawer->velodyne_pose, v_drawer->sensor_board_pose);
double shot_time = last_timestamp + ((velodyne_message->timestamp - last_timestamp)*((double)i)/((double)velodyne_message->number_of_32_laser_shots));
carmen_vector_3D_t car_interpolated_position = carmen_get_interpolated_robot_position_at_time(car_fused_pose, car_fused_velocity, car_fused_time, shot_time, r_matrix_car_to_global);
carmen_vector_3D_t point_global_position = get_point_position_global_reference(car_interpolated_position, point_position, r_matrix_car_to_global);
velodyne_points.points[k] = point_global_position;
velodyne_points.point_color[k] = create_point_colors_intensity(velodyne_message->partial_scan[i].intensity[j]);
k++;
}
}
}
}
velodyne_points.num_points = k;
normalize_intensity(velodyne_points);
destroy_rotation_matrix(velodyne_to_board_matrix);
destroy_rotation_matrix(board_to_car_matrix);
destroy_rotation_matrix(r_matrix_car_to_global);
add_point_cloud(v_drawer->pcloud_drawer, velodyne_points);
last_timestamp = velodyne_message->timestamp;
}
