float vect_get_angle(vect_t *v, vect_t *w)
{
float ps;
float n;
ps = vect_pscal(v, w);
n = vect_norm(v) * vect_norm(w);
return acosf((float)ps/n);
}
//Gives a ratio of the length of two vectors. The use of this function is
//meaningful only if the two vectors are (nearly) collinear
double vect_divide(double* A, double* B, int n) {
double ratio;
if(dot_product(A,B,n)>=0) {
ratio=vect_norm(A,n)/vect_norm(B,n);
}
else {
ratio=-vect_norm(A,n)/vect_norm(B,n);
}
return ratio;
}
void vect_resize(vect_t *v, float l)
{
float old_l = vect_norm(v);
float x = v->x, y = v->y;
v->x = x * l / old_l;
v->y = y * l / old_l;
}
void gen_vertex_normals(MESH *m)
{
int i;
LLIST *vert = malloc(sizeof(LLIST)*m->nv);
memset(vert, 0, sizeof(LLIST)*m->nv);
LLIST *tmp;
int index;
// tell each vert about it's faces
for(i=0; i<m->nf; i++)
{
index = m->f[i].x;
tmp = malloc(sizeof(LLIST));
tmp->face = i;
tmp->next = vert[index].next;
vert[index].next = tmp;
index = m->f[i].y;
tmp = malloc(sizeof(LLIST));
tmp->face = i;
tmp->next = vert[index].next;
vert[index].next = tmp;
index = m->f[i].z;
tmp = malloc(sizeof(LLIST));
tmp->face = i;
tmp->next = vert[index].next;
vert[index].next = tmp;
}
float3 *no = malloc(sizeof(float3)*m->nv); // normal out
float3 t;
// now average the normals and store in the output
for(i=0; i<m->nv; i++)
{
tmp = vert[i].next;
t.x = t.y = t.z = 0;
while(tmp)
{
vect_add( &t, &t, &m->n[tmp->face]);
LLIST *last = tmp;
tmp = tmp->next;
free(last);
}
vect_norm(&no[i], &t);
}
free(vert);
free(m->n);
m->n = no;
m->nn = m->nv;
}
void gen_vertex_normals(MESH *m)
{
ListInt *vert = malloc(sizeof(ListInt)*m->nv);
memset(vert, 0, sizeof(ListInt)*m->nv);
// tell each vert about it's faces
for(int i=0; i<m->nf; i++)
{
int index = m->f[i].x;
ListInt *tmp = malloc(sizeof(ListInt));
tmp->x = i;
tmp->next = vert[index].next;
vert[index].next = tmp;
index = m->f[i].y;
tmp = malloc(sizeof(ListInt));
tmp->x = i;
tmp->next = vert[index].next;
vert[index].next = tmp;
index = m->f[i].z;
tmp = malloc(sizeof(ListInt));
tmp->x = i;
tmp->next = vert[index].next;
vert[index].next = tmp;
}
float3 *no = malloc(sizeof(float3)*m->nv); // normal out
// now average the normals and store in the output
for(int i=0; i<m->nv; i++)
{
ListInt *tmp = vert[i].next;
float3 t = { 0, 0, 0 };
while(tmp)
{
vect_add( &t, &t, &m->n[tmp->x]);
ListInt *last = tmp;
tmp = tmp->next;
free(last);
}
vect_norm(&no[i], &t);
}
free(vert);
free(m->n);
m->n = no;
m->nn = m->nv;
}
int pt_lighted(t_obj *obj, t_point pt, t_light *light)
{
t_ray ray;
float dist_to_light;
ray.orig.x = pt.x;
ray.orig.y = pt.y;
ray.orig.z = pt.z;
ray.dir.x = light->orig.x - ray.oriug.x;
ray.dir.y = light->orig.y - ray.oriug.y;
ray.dir.z = light->orig.z - ray.oriug.z;
dist_to_light = vect_norm(&ray.dir);
normalize_vect(&ray, 0, obj, 0);
if (ray.inter_t <= dist_ti_light)
return (0);
return (1);
}
uint8_t
radar_blocking (const vect_t *robot_pos, const vect_t *dest_pos,
const vect_t *obs_pos, uint8_t obs_nb)
{
uint8_t i;
/* Stop here if no obstacle. */
if (!obs_nb)
return 0;
vect_t vd = *dest_pos;
vect_sub (&vd, robot_pos);
uint16_t d = vect_norm (&vd);
/* If destination is realy near, stop here. */
if (d < RADAR_EPSILON_MM)
return 0;
/* If destination is near, use clearance to destination point instead of
* stop length. */
vect_t t;
if (d < RADAR_STOP_MM)
t = *dest_pos;
else
{
vect_scale_f824 (&vd, (1ll << 24) / d * RADAR_STOP_MM);
t = *robot_pos;
vect_translate (&t, &vd);
}
/* Now, look at obstacles. */
for (i = 0; i < obs_nb; i++)
{
/* Vector from robot to obstacle. */
vect_t vo = obs_pos[i];
vect_sub (&vo, robot_pos);
/* Ignore if in our back. */
int32_t dp = vect_dot_product (&vd, &vo);
if (dp < 0)
continue;
/* Check distance. */
int16_t od = distance_segment_point (robot_pos, &t, &obs_pos[i]);
if (od > BOT_SIZE_SIDE + RADAR_CLEARANCE_MM / 2
+ RADAR_OBSTACLE_RADIUS_MM)
continue;
/* Else, obstacle is blocking. */
return 1;
}
return 0;
}
void gen_normals(MESH* m)
{
int i;
float3 a, b, t;
float3 *n = malloc(sizeof(float3)*m->nf);
// Generate per face normals
for(i=0; i<m->nf; i++)
{
vect_sub( &a, &m->v[m->f[i].x], &m->v[m->f[i].y] );
vect_sub( &b, &m->v[m->f[i].x], &m->v[m->f[i].z] );
vect_cross( &t, &b, &a );
vect_norm( &n[i], &t);
}
m->n = n;
m->nn = m->nf;
}
/* Fill the 2 circles pointer given as parameter, each of those cross
* both beacons b1 and b2. From any point of these circles (except b1
* and b2), we see b1 and b2 with the angle of a_rad (which must be
* positive). Return 0 on success.
*
* l
* <------------------------->
*
* b1 O b2
* +----------------------------+
* ,' \\ | /|'\
* / \ \ | ^ / | `
* / \ \ a___ | | d / | `.
* / \ \ / | v / | \
* | \ \ | / | |
* | \ \ | / | |
* | \ \|/ | |
* | \ * C | |
* | \ | .'
* | \ | |
* | \ | .'
* \ \ a____| /
* \ \ / | ,'
* ` \ | /
* '. \ | ,'
* '-. \ |_,'
* '-._ _,*'
* '`--......---' R (the robot)
*
*/
int8_t angle_to_circles(circle_t *c1, circle_t *c2,
const point_t *b1, const point_t *b2,
double a_rad)
{
point_t O;
vect_t v;
float l, d;
/* reject too small angles */
if (a_rad <= 0.01 && a_rad >= -0.01)
return -1;
/* get position of O */
O.x = (b1->x + b2->x) / 2;
O.y = (b1->y + b2->y) / 2;
/* get the length l */
v.x = b2->x - b1->x;
v.y = b2->y - b1->y;
l = vect_norm(&v);
/* distance from O to the center of the circle */
/* XXX div by 0 when pi */
d = l / (2 * tan(a_rad));
/* get the circle c1 */
vect_rot_trigo(&v);
vect_resize(&v, d);
if (c1) {
c1->x = O.x + v.x;
c1->y = O.y + v.y;
c1->r = xy_norm(b1->x, b1->y, c1->x, c1->y);
}
/* get the circle c2 */
if (c2) {
c2->x = O.x - v.x;
c2->y = O.y - v.y;
c2->r = xy_norm(b1->x, b1->y, c1->x, c1->y);
}
return 0;
}
void attitude_tobi_laurens(void)
{
//Transform accelerometer used in all directions
// float_vect3 acc_nav;
//body2navi(&global_data.accel_si, &global_data.attitude, &acc_nav);
// Kalman Filter
//Calculate new linearized A matrix
attitude_tobi_laurens_update_a();
kalman_predict(&attitude_tobi_laurens_kal);
//correction update
m_elem measurement[9] =
{ };
m_elem mask[9] =
{ 1.0f, 1.0f, 1.0f, 0, 0, 0, 1.0f, 1.0f, 1.0f };
float_vect3 acc;
float_vect3 mag;
float_vect3 gyro;
// acc.x = global_data.accel_raw.x * 9.81f /690;
// acc.y = global_data.accel_raw.y * 9.81f / 690;
// acc.z = global_data.accel_raw.z * 9.81f / 690;
#ifdef IMU_PIXHAWK_V250
acc.x = global_data.accel_raw.x * (650.0f/900.0f);
acc.y = global_data.accel_raw.y * (650.0f/900.0f);
acc.z = global_data.accel_raw.z * (650.0f/900.0f);
#else
acc.x = global_data.accel_raw.x;
acc.y = global_data.accel_raw.y;
acc.z = global_data.accel_raw.z;
#endif
float acc_norm = sqrt(global_data.accel_raw.x * global_data.accel_raw.x + global_data.accel_raw.y * global_data.accel_raw.y + global_data.accel_raw.z * global_data.accel_raw.z);
static float acc_norm_filt = SCA3100_COUNTS_PER_G;
float acc_norm_lp = 0.05f;
acc_norm_filt = (1.0f - acc_norm_lp) * acc_norm_filt + acc_norm_lp
* acc_norm;
// static float acc_norm_filtz = SCA3100_COUNTS_PER_G;
// float acc_norm_lpz = 0.05;
// acc_norm_filtz = (1.0f - acc_norm_lpz) * acc_norm_filtz + acc_norm_lpz * -acc.z;
float acc_diff = fabs(acc_norm_filt - SCA3100_COUNTS_PER_G);
if (acc_diff > 200.0f)
{
//Don't correct when acc high
mask[0] = 0;
mask[1] = 0;
mask[2] = 0;
}
else if (acc_diff > 100.0f)
{
//fade linearely out between 100 and 200
float mask_lin = (200.0f - acc_diff) / 100.0f;
mask[0] = mask_lin;
mask[1] = mask_lin;
mask[2] = mask_lin;
}
//else mask stays 1
// static uint8_t i = 0;
// if (i++ > 10)
// {
// i = 0;
// float_vect3 debug;
// debug.x = mask[0];
// debug.y = acc_norm;
// debug.z = acc_norm_filt;
// debug_vect("acc_norm", debug);
// }
// mag.x = (global_data.magnet_corrected.x ) * 1.f / 510.f;
// mag.y = (global_data.magnet_corrected.y) * 1.f / 510.f;
// mag.z = (global_data.magnet_corrected.z) * 1.f / 510.f;
mag.x = (global_data.magnet_corrected.x ) ;
mag.y = (global_data.magnet_corrected.y) ;
mag.z = (global_data.magnet_corrected.z) ;
// gyro.x = -(global_data.gyros_raw.x-global_data.param[PARAM_GYRO_OFFSET_X]) * 0.000955;
// gyro.y = (global_data.gyros_raw.y-global_data.param[PARAM_GYRO_OFFSET_Y]) * 0.000955;
// gyro.z = -(global_data.gyros_raw.z-global_data.param[PARAM_GYRO_OFFSET_Z]) * 0.001010;
gyro.x = -(global_data.gyros_raw.x-global_data.param[PARAM_GYRO_OFFSET_X]) * 0.001008f;
gyro.y = (global_data.gyros_raw.y-global_data.param[PARAM_GYRO_OFFSET_Y]) * 0.001008f;
gyro.z = -(global_data.gyros_raw.z-global_data.param[PARAM_GYRO_OFFSET_Z]) * 0.001010f;
measurement[0] = acc.x;
measurement[1] = acc.y;
measurement[2] = acc.z;
if (global_data.state.yaw_estimation_mode == YAW_ESTIMATION_MODE_VISION)
{
measurement[3] = global_data.vision_magnetometer_replacement.x;
measurement[4] = global_data.vision_magnetometer_replacement.y;
measurement[5] = global_data.vision_magnetometer_replacement.z;
}
else if (global_data.state.yaw_estimation_mode == YAW_ESTIMATION_MODE_GLOBAL_VISION)
{
// measurement[3] = global_data.vision_global_magnetometer_replacement.x;
// measurement[4] = global_data.vision_global_magnetometer_replacement.y;
// measurement[5] = global_data.vision_global_magnetometer_replacement.z;
}
else if (global_data.state.yaw_estimation_mode == YAW_ESTIMATION_MODE_VICON)
{
measurement[3] = global_data.vicon_magnetometer_replacement.x;
measurement[4] = global_data.vicon_magnetometer_replacement.y;
measurement[5] = global_data.vicon_magnetometer_replacement.z;
}
else if (global_data.state.yaw_estimation_mode == YAW_ESTIMATION_MODE_INTEGRATION)
{
// Do nothing
// KEEP THIS IN HERE
}
else if (global_data.state.yaw_estimation_mode == YAW_ESTIMATION_MODE_MAGNETOMETER) // YAW_ESTIMATION_MODE_MAGNETOMETER
{
measurement[3] = mag.x;
measurement[4] = mag.y;
measurement[5] = mag.z;
// debug_vect("mag_f", mag);
}
else
{
static uint8_t errcount = 0;
if (errcount == 0) debug_message_buffer("ATT EST. ERROR: No valid yaw estimation mode set!");
errcount++;
global_data.state.status = MAV_STATE_CRITICAL;
}
measurement[6] = gyro.x;
measurement[7] = gyro.y;
measurement[8] = gyro.z;
//Put measurements into filter
static int j = 0;
// MASK
if (global_data.state.yaw_estimation_mode == YAW_ESTIMATION_MODE_INTEGRATION)
{
// Do nothing, pure integration
}
else if (global_data.state.yaw_estimation_mode == YAW_ESTIMATION_MODE_VICON)
{
// If our iteration count is right and new vicon data is available, update measurement
if (j >= 3 && global_data.state.vicon_attitude_new_data == 1)
{
j = 0;
mask[3]=1;
mask[4]=1;
mask[5]=1;
}
else
{
j++;
}
global_data.state.vicon_attitude_new_data = 0;
}
else if (global_data.state.yaw_estimation_mode == YAW_ESTIMATION_MODE_VISION)
{
// If the iteration count is right and new vision data is available, update measurement
if (j >= 3 && global_data.state.vision_attitude_new_data == 1)
{
j = 0;
mask[3]=1;
mask[4]=1;
mask[5]=1;
}
else
{
j++;
}
global_data.state.vision_attitude_new_data = 0;
}
else if (global_data.state.yaw_estimation_mode == YAW_ESTIMATION_MODE_GLOBAL_VISION)
{
// // If the iteration count is right and new vision data is available, update measurement
// if (j >= 3 && global_data.state.global_vision_attitude_new_data == 1)
// {
// j = 0;
//
// mask[3]=1;
// mask[4]=1;
// mask[5]=1;
// j=0;
// }
// else
// {
// j++;
// }
// global_data.state.global_vision_attitude_new_data = 0;
}
else
{
if (j >= 3 && global_data.state.magnet_ok)
{
j = 0;
mask[3]=1;
mask[4]=1;
mask[5]=1;
}
else
{
j++;
}
}
kalman_correct(&attitude_tobi_laurens_kal, measurement, mask);
//debug
// save outputs
float_vect3 kal_acc, kal_mag, kal_w0, kal_w;
kal_acc.x = kalman_get_state(&attitude_tobi_laurens_kal, 0);
kal_acc.y = kalman_get_state(&attitude_tobi_laurens_kal, 1);
kal_acc.z = kalman_get_state(&attitude_tobi_laurens_kal, 2);
kal_mag.x = kalman_get_state(&attitude_tobi_laurens_kal, 3);
kal_mag.y = kalman_get_state(&attitude_tobi_laurens_kal, 4);
kal_mag.z = kalman_get_state(&attitude_tobi_laurens_kal, 5);
kal_w0.x = kalman_get_state(&attitude_tobi_laurens_kal, 6);
kal_w0.y = kalman_get_state(&attitude_tobi_laurens_kal, 7);
kal_w0.z = kalman_get_state(&attitude_tobi_laurens_kal, 8);
kal_w.x = kalman_get_state(&attitude_tobi_laurens_kal, 9);
kal_w.y = kalman_get_state(&attitude_tobi_laurens_kal, 10);
kal_w.z = kalman_get_state(&attitude_tobi_laurens_kal, 11);
float_vect3 x_n_b, y_n_b, z_n_b;
z_n_b.x = -kal_acc.x;
z_n_b.y = -kal_acc.y;
z_n_b.z = -kal_acc.z;
vect_norm(&z_n_b);
vect_cross_product(&z_n_b, &kal_mag, &y_n_b);
vect_norm(&y_n_b);
vect_cross_product(&y_n_b, &z_n_b, &x_n_b);
//save euler angles
global_data.attitude.x = atan2f(z_n_b.y, z_n_b.z);
global_data.attitude.y = -asinf(z_n_b.x);
if (global_data.state.yaw_estimation_mode == YAW_ESTIMATION_MODE_INTEGRATION)
{
global_data.attitude.z += 0.005f * global_data.gyros_si.z;
}
else if (global_data.state.yaw_estimation_mode == YAW_ESTIMATION_MODE_GLOBAL_VISION)
{
global_data.attitude.z += 0.0049f * global_data.gyros_si.z;
if (global_data.state.global_vision_attitude_new_data == 1)
{
global_data.attitude.z = 0.995f*global_data.attitude.z + 0.005f*global_data.vision_data_global.ang.z;
// Reset new data flag at roughly 1 Hz, detecting a vision timeout
static uint8_t new_data_reset = 0;
if (new_data_reset == 0)
{
global_data.state.global_vision_attitude_new_data = 0;
}
new_data_reset++;
}
}
else
{
// static hackMagLowpass = 0.0f;
global_data.attitude.z = global_data.attitude.z*0.9f+0.1f*atan2f(y_n_b.x, x_n_b.x);
}
//save rates
global_data.attitude_rate.x = kal_w.x;
global_data.attitude_rate.y = kal_w.y;
global_data.attitude_rate.z = kal_w.z;
global_data.yaw_lowpass = 0.99f * global_data.yaw_lowpass + 0.01f * global_data.attitude_rate.z;
}
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 test_basic_vector_op(void)
{
/******************************/
/* Test the vector operations */
/******************************/
/*create two vectors and show them*/
int vector_length = 5;
double vector_a[5] = {2.5, -10.9, 15.8, 12.2, 7.9};
double vector_b[5] = {2.5, 0.2, 21.33, 70.1, -0.2};
double vector_c[5];
double result = 0.0;
/*seed the random generator*/
srand(time(NULL));
printf("The two vectors a and b\n");
show_vector(vector_a, vector_length);
show_vector(vector_b, vector_length);
/*vector addition*/
printf("c = a + b\n");
/*copy a*/
memcpy(vector_c, vector_a, vector_length*sizeof(double));
/*compute*/
vect_add(vector_c, vector_b, vector_length);
/*show expected and obtained results*/
printf("Expected: 5.0000 -10.7000 37.1300 82.3000 7.7000\n");
printf("Result: ");
show_vector(vector_c, vector_length);
/*vector subtraction*/
printf("c = a - b\n");
/*copy a*/
memcpy(vector_c, vector_a, vector_length*sizeof(double));
/*compute*/
vect_sub(vector_c, vector_b, vector_length);
/*show expected and obtained results*/
printf("Expected: 0.0000 -11.1000 -5.5300 -57.9000 8.1000\n");
printf("Result: ");
show_vector(vector_c, vector_length);
/*vector scalar multiplication*/
printf("c = a * 5\n");
/*copy a*/
memcpy(vector_c, vector_a, vector_length*sizeof(double));
/*compute*/
vect_scalar_multiply(vector_c, 5, vector_length);
/*show expected and obtained results*/
printf("Expected: 12.5000 -54.5000 79.0000 61.0000 39.5000\n");
printf("Result: ");
show_vector(vector_c, vector_length);
/*vector dot product*/
printf("c = a .* b\n");
/*compute*/
result = vect_dot_product(vector_a, vector_b, vector_length);
/*show expected and obtained results*/
printf("Expected: 1194.72\n");
printf("Result: %2.2f\n",result);
/*vector cross product*/
/*To be completed*/
/*vector norm*/
printf("c = |a|\n");
/*compute*/
result = vect_norm(vector_a, vector_length);
/*show expected and obtained results*/
printf("Expected: 24.2064\n");
printf("Result: %2.4f\n",result);
/*vector rand unit*/
printf("c is a random vector\n");
printf("|c| == 1\n");
/*compute*/
vect_rand_unit(vector_c, vector_length);
/*show expected and obtained results*/
printf("The vector: ");
show_vector(vector_c, vector_length);
result = vect_norm(vector_c, vector_length);
printf("Expected norm: 1.0000\n");
printf("Obtained norm: %2.4f\n",result);
}
/**
* void mtx_lanczos_procedure(double *A, double *Tm, int n, int m)
*
* @brief Function that implements the first step of the eigenproblem solution
* based on lanzos algorithm. This function generates a matrix Tm that contains
* a set (<=) of eigenvalues that approximate those of matrix A. Finding the eigenvalues
* in Tm is easier and serves as an optimization method for problems in which only a few
* eigenpairs are required.
* @param A, the matrix on which the procedure is applied. Needs to be square and Hermitian.
* @param (out)a, elements of the diagonal of the matrix Tm
* @param (out)b, elements off diagonal of the matrix Tm
* @param n, the dimensions of the square matrix A
* @param m, the number of iterations for the lanczos procedure (and the dimensions of the array returned)
*/
void mtx_lanczos_procedure(double *A, double *a, double *b, int n, int m)
{
int i,j,array_index_i;
double* v_i = (double*)calloc(n, sizeof(double));
double* a_times_v_i = (double*)calloc(n, sizeof(double));
double* b_times_v_i_minus_one = (double*)calloc(n, sizeof(double));
double* v_i_minus_one = (double*)calloc(n, sizeof(double));
double* w_i = (double*)calloc(n, sizeof(double));
double* v1 = (double*)calloc(n, sizeof(double));
double* temp;
/*get a n-long random vector with norm equal to 1*/
//vect_rand_unit(v1,n);
for(i=0;i<n;i++){
v1[i] = 1/sqrt(n);
}
/*first iteration of the procedure*/
i = 1;
array_index_i = i-1;
/*computes w_i*/
/*w[i] <= A*v[i]*/
mtx_mult(A, v1, w_i, n, n, 1);
/*computes a_i*/
/*a[i] <= w[i]*v[i]*/
a[array_index_i] = vect_dot_product(w_i, v1,n);
/*update w_i*/
/*w[i] <= w[i]-a[i]*v[i]-b[i]*v[i-1]*/
/*note: b[1] = 0*/
memcpy(a_times_v_i,v1,n*sizeof(double));
vect_scalar_multiply(a_times_v_i,a[array_index_i],n);
vect_sub(w_i, a_times_v_i, n);
/*computes next b_i*/
/*b[i+1] = ||w[i]||*/
b[array_index_i+1] = vect_norm(w_i,n);
/*copy v1 to v_i*/
memcpy(v_i,v1,n*sizeof(double));
/*save v[i] to be used asv[i-1]*/
temp = v_i_minus_one;
v_i_minus_one = v_i;
/*computes next v_i = w_i/b(i+1)*/
/*v[i+1] = w[i]/b[i+1]*/
v_i = w_i;
vect_scalar_multiply(v_i, 1/b[array_index_i+1], n);
/*reuse memory former v_i_minus_one space for w_i*/
w_i = temp;
/*rest of the iterations of the procedure*/
for(i=2;i<=m-1;i++){
array_index_i = i-1;
/*computes w_i*/
/*w[i] <= A*v[i]*/
mtx_mult(A, v_i, w_i, n, n, 1);
/*computes a_i*/
/*a[i] <= w[i]*v[i]*/
a[array_index_i] = vect_dot_product(w_i, v_i, n);
/*update w_i*/
/*prepare a[i]*v[i]*/
memcpy(a_times_v_i,v_i,n*sizeof(double));
vect_scalar_multiply(a_times_v_i,a[array_index_i],n);
/*prepare b[i]*v[i-1]*/
memcpy(b_times_v_i_minus_one,v_i_minus_one,n*sizeof(double));
vect_scalar_multiply(b_times_v_i_minus_one,b[array_index_i],n);
/*w[i] <= w[i]-a[i]*v[i]-b[i]*v[i-1]*/
vect_sub(w_i, a_times_v_i,n);
vect_sub(w_i, b_times_v_i_minus_one,n);
/*computes next b_i*/
/*b[i+1] = ||w[i]||*/
b[array_index_i+1] = vect_norm(w_i,n);
/*save current v_i*/
temp = v_i_minus_one;
v_i_minus_one = v_i;
/*computes next v_i = w_i/b(i+1)*/
/*v[i+1] = w[i]/b[i+1]*/
v_i = w_i;
vect_scalar_multiply(v_i, 1/b[array_index_i+1],n);
/*reuse memory former v_i_minus_one space for w_i*/
w_i = temp;
}
/*compute last term a[m]*/
array_index_i = m-1;
mtx_mult(A, v_i, w_i, n, n, 1);
a[array_index_i] = vect_dot_product(w_i,v_i,n);
/*translate data representation in the b matrix to match the CLAPACK standard*/
for(i=0;i<(n-1);i++){
b[i] = b[i+1];
}
b[n-1] = 0;
free(v1);
free(v_i);
free(a_times_v_i);
free(b_times_v_i_minus_one);
free(v_i_minus_one);
free(w_i);
}
