quaternion_s make_look_quaternion(vect_s direction, vect_s up)
{
vect_s i = vect_normalize(direction);
vect_s j = vect_normalize(vect_sub(up, vect_project(up, direction)));
vect_s k = vect_cross(i, j);
return make_quaternion_from_ijk(i, j, k);
}
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;
}
int AD_PatchObject::Tassellate_NormalsTexture(void)
{
// ***************************************************************
// TASSELLIZZAZIONE con generazione anche delle normali ai vertici
// e coordinate texture
// ***************************************************************
AD_Vertex3D *vertex_row_up, *vertex_row_down;
AD_Vect3D *points_already_calculated, *points_to_calculate;
AD_VectUV *UVpoints_already_calculated, *UVpoints_to_calculate;
AD_Vect3D *normals_already_calculated, *normals_to_calculate;
float4 u_step, v_step, u, v;
float4 tu0, tu1, tv0, tv1, dtu, dtv;
int num_u_step, num_v_step, w, i, j;
int ntria=0;
AD_Vect3D p1, p2;
u_step=1.0f/(u_evaluations-1); num_u_step=(int)(u_evaluations);
v_step=1.0f/(v_evaluations-1); num_v_step=(int)(v_evaluations);
points_already_calculated=&points_tr[0];
points_to_calculate=&points_tr[num_u_step];
vertex_row_up=&vertex3D[0];
vertex_row_down=&vertex3D[num_u_step];
UVpoints_already_calculated=&vertexUV[0];
UVpoints_to_calculate=&vertexUV[num_u_step];
normals_already_calculated=&normals[0];
normals_to_calculate=&normals[num_u_step];
ntria=0;
for (w=0; w<num_patches; w++)
{
// precalcolo fila iniziale (isoparametrica v=0)
v=u=0;
dtu=(UVverteces[patches[w].UVvert[3]].u-
UVverteces[patches[w].UVvert[0]].u)/(u_evaluations-1);
dtv=(UVverteces[patches[w].UVvert[3]].v-
UVverteces[patches[w].UVvert[0]].v)/(u_evaluations-1);
tu0=UVverteces[patches[w].UVvert[0]].u;
tv0=UVverteces[patches[w].UVvert[0]].v;
for (i=0; i<num_u_step; i++)
{
Evaluate_Patch(&patches[w], u, 0, &points_already_calculated[i]);
Evaluate_uDerivate(&patches[w], u, 0, &p1);
Evaluate_vDerivate(&patches[w], u, 0, &p2);
vect_cross(&p1, &p2, &normals_already_calculated[i]);
vect_normalize(&normals_already_calculated[i]);
UVpoints_already_calculated[i].u=tu0;
UVpoints_already_calculated[i].v=tv0;
tu0+=dtu;
tv0+=dtv;
u+=u_step;
}
v=0;
for (j=0; j<num_v_step-1; j++)
{
v+=v_step;
u=0;
tu0= v*UVverteces[patches[w].UVvert[1]].u+
(1-v)*UVverteces[patches[w].UVvert[0]].u;
tv0= v*UVverteces[patches[w].UVvert[1]].v+
(1-v)*UVverteces[patches[w].UVvert[0]].v;
tu1= v*UVverteces[patches[w].UVvert[2]].u+
(1-v)*UVverteces[patches[w].UVvert[3]].u;
tv1= v*UVverteces[patches[w].UVvert[2]].v+
(1-v)*UVverteces[patches[w].UVvert[3]].v;
dtu=(tu1-tu0)/(u_evaluations-1);
dtv=(tv1-tv0)/(u_evaluations-1);
for (i=0; i<num_u_step; i++)
{
Evaluate_Patch(&patches[w], u, v, &points_to_calculate[i]);
Evaluate_uDerivate(&patches[w], u, v, &p1);
Evaluate_vDerivate(&patches[w], u, v, &p2);
vect_cross(&p1, &p2, &normals_to_calculate[i]);
vect_normalize(&normals_to_calculate[i]);
UVpoints_to_calculate[i].u=tu0;
UVpoints_to_calculate[i].v=tv0;
tu0+=dtu;
tv0+=dtv;
u+=u_step;
}
// creazione dei triangoli
for (i=0; i<num_u_step-1; i++)
{
tria[ntria].v1=vertex_row_up;
tria[ntria].v2=&(*(vertex_row_up+1));
tria[ntria].v3=&(*(vertex_row_down+1));
tria[ntria].v1->tpoint=&points_already_calculated[i];
tria[ntria].v2->tpoint=&points_already_calculated[i+1];
tria[ntria].v3->tpoint=&points_to_calculate[i+1];
tria[ntria].v1->normal=&normals_already_calculated[i];
tria[ntria].v2->normal=&normals_already_calculated[i+1];
tria[ntria].v3->normal=&normals_to_calculate[i+1];
// calcolo della normale
vect_sub(tria[ntria].v1->tpoint, tria[ntria].v2->tpoint, &p1);
vect_sub(tria[ntria].v3->tpoint, tria[ntria].v2->tpoint, &p2);
vect_cross(&p1, &p2, &tria[ntria].normal);
vect_normalize(&tria[ntria].normal);
tria[ntria].uv1=&UVpoints_already_calculated[i];
tria[ntria].uv2=&UVpoints_already_calculated[i+1];
tria[ntria].uv3=&UVpoints_to_calculate[i+1];
ntria++;
tria[ntria].v1=&(*(vertex_row_down+1));
tria[ntria].v2=vertex_row_down;
tria[ntria].v3=vertex_row_up;
tria[ntria].v1->tpoint=&points_to_calculate[i+1];
tria[ntria].v2->tpoint=&points_to_calculate[i];
tria[ntria].v3->tpoint=&points_already_calculated[i];
tria[ntria].v1->normal=&normals_to_calculate[i+1];
tria[ntria].v2->normal=&normals_to_calculate[i];
tria[ntria].v3->normal=&normals_already_calculated[i];
// calcolo della normale
vect_sub(tria[ntria].v1->tpoint, tria[ntria].v2->tpoint, &p1);
vect_sub(tria[ntria].v3->tpoint, tria[ntria].v2->tpoint, &p2);
vect_cross(&p1, &p2, &tria[ntria].normal);
vect_normalize(&tria[ntria].normal);
tria[ntria].uv1=&UVpoints_to_calculate[i+1];
tria[ntria].uv2=&UVpoints_to_calculate[i];
tria[ntria].uv3=&UVpoints_already_calculated[i];
ntria++;
vertex_row_up+=1;
vertex_row_down+=1;
}
vertex_row_up+=1;
vertex_row_down+=1;
points_already_calculated+=num_u_step;
points_to_calculate+=num_u_step;
normals_already_calculated+=num_u_step;
normals_to_calculate+=num_u_step;
UVpoints_already_calculated+=num_u_step;
UVpoints_to_calculate+=num_u_step;
}
vertex_row_up+=num_u_step;
vertex_row_down+=num_u_step;
points_already_calculated+=num_u_step;
points_to_calculate+=num_u_step;
normals_already_calculated+=num_u_step;
normals_to_calculate+=num_u_step;
UVpoints_already_calculated+=num_u_step;
UVpoints_to_calculate+=num_u_step;
}
return(ntria);
}
int AD_PatchObject::Tassellate_Normals(void)
{
// ***************************************************************
// TASSELLIZZAZIONE con generazione anche delle normali ai vertici
// ***************************************************************
AD_Vertex3D *vertex_row_up, *vertex_row_down;
AD_Vect3D *points_already_calculated, *points_to_calculate;
AD_Vect3D *normals_already_calculated, *normals_to_calculate;
float4 u_step, v_step, u, v;
int num_u_step, num_v_step, w, i, j;
int ntria=0;
AD_Vect3D p1, p2;
u_step=1.0f/(u_evaluations-1); num_u_step=(int)(u_evaluations);
v_step=1.0f/(v_evaluations-1); num_v_step=(int)(v_evaluations);
points_already_calculated=&points_tr[0];
points_to_calculate=&points_tr[num_u_step];
vertex_row_up=&vertex3D[0];
vertex_row_down=&vertex3D[num_u_step];
normals_already_calculated=&normals[0];
normals_to_calculate=&normals[num_u_step];
ntria=0;
for (w=0; w<num_patches; w++)
{
// precalcolo fila iniziale (isoparametrica v=0)
v=u=0;
for (i=0; i<num_u_step; i++)
{
Evaluate_Patch(&patches[w], u, 0, &points_already_calculated[i]);
Evaluate_uDerivate(&patches[w], u, 0, &p1);
Evaluate_vDerivate(&patches[w], u, 0, &p2);
vect_cross(&p1, &p2, &normals_already_calculated[i]);
vect_normalize(&normals_already_calculated[i]);
u+=u_step;
}
v=0;
for (j=0; j<num_v_step-1; j++)
{
v+=v_step;
u=0;
for (i=0; i<num_u_step; i++)
{
Evaluate_Patch(&patches[w], u, v, &points_to_calculate[i]);
Evaluate_uDerivate(&patches[w], u, v, &p1);
Evaluate_vDerivate(&patches[w], u, v, &p2);
vect_cross(&p1, &p2, &normals_to_calculate[i]);
vect_normalize(&normals_to_calculate[i]);
u+=u_step;
}
// creazione dei triangoli
for (i=0; i<num_u_step-1; i++)
{
tria[ntria].v1=vertex_row_up;
tria[ntria].v2=&(*(vertex_row_up+1));
tria[ntria].v3=&(*(vertex_row_down+1));
tria[ntria].v1->tpoint=&points_already_calculated[i];
tria[ntria].v2->tpoint=&points_already_calculated[i+1];
tria[ntria].v3->tpoint=&points_to_calculate[i+1];
tria[ntria].v1->normal=&normals_already_calculated[i];
tria[ntria].v2->normal=&normals_already_calculated[i+1];
tria[ntria].v3->normal=&normals_to_calculate[i+1];
// calcolo della normale
vect_sub(tria[ntria].v1->tpoint, tria[ntria].v2->tpoint, &p1);
vect_sub(tria[ntria].v3->tpoint, tria[ntria].v2->tpoint, &p2);
vect_cross(&p1, &p2, &tria[ntria].normal);
vect_normalize(&tria[ntria].normal);
ntria++;
tria[ntria].v1=&(*(vertex_row_down+1));
tria[ntria].v2=vertex_row_down;
tria[ntria].v3=vertex_row_up;
tria[ntria].v1->tpoint=&points_to_calculate[i+1];
tria[ntria].v2->tpoint=&points_to_calculate[i];
tria[ntria].v3->tpoint=&points_already_calculated[i];
tria[ntria].v1->normal=&normals_to_calculate[i+1];
tria[ntria].v2->normal=&normals_to_calculate[i];
tria[ntria].v3->normal=&normals_already_calculated[i];
// calcolo della normale
vect_sub(tria[ntria].v1->tpoint, tria[ntria].v2->tpoint, &p1);
vect_sub(tria[ntria].v3->tpoint, tria[ntria].v2->tpoint, &p2);
vect_cross(&p1, &p2, &tria[ntria].normal);
vect_normalize(&tria[ntria].normal);
ntria++;
vertex_row_up+=1;
vertex_row_down+=1;
}
vertex_row_up+=1;
vertex_row_down+=1;
points_already_calculated+=num_u_step;
points_to_calculate+=num_u_step;
normals_already_calculated+=num_u_step;
normals_to_calculate+=num_u_step;
}
vertex_row_up+=num_u_step;
vertex_row_down+=num_u_step;
points_already_calculated+=num_u_step;
points_to_calculate+=num_u_step;
normals_already_calculated+=num_u_step;
normals_to_calculate+=num_u_step;
}
return(ntria);
}
/*
Decodes a 3x4 transformation matrix into separate scale, rotation,
translation, and shear vectors. Based on a program by Spencer W.
Thomas (Graphics Gems II)
*/
void mat_decode (Matrix mat, Vector scale, Vector shear, Vector rotate,
Vector transl)
{
int i;
Vector row[3], temp;
for (i = 0; i < 3; i++)
transl[i] = mat[3][i];
for (i = 0; i < 3; i++) {
row[i][X] = mat[i][0];
row[i][Y] = mat[i][1];
row[i][Z] = mat[i][2];
}
scale[X] = vect_mag (row[0]);
vect_normalize (row[0]);
shear[X] = vect_dot (row[0], row[1]);
row[1][X] = row[1][X] - shear[X]*row[0][X];
row[1][Y] = row[1][Y] - shear[X]*row[0][Y];
row[1][Z] = row[1][Z] - shear[X]*row[0][Z];
scale[Y] = vect_mag (row[1]);
vect_normalize (row[1]);
if (scale[Y] != 0.0)
shear[X] /= scale[Y];
shear[Y] = vect_dot (row[0], row[2]);
row[2][X] = row[2][X] - shear[Y]*row[0][X];
row[2][Y] = row[2][Y] - shear[Y]*row[0][Y];
row[2][Z] = row[2][Z] - shear[Y]*row[0][Z];
shear[Z] = vect_dot (row[1], row[2]);
row[2][X] = row[2][X] - shear[Z]*row[1][X];
row[2][Y] = row[2][Y] - shear[Z]*row[1][Y];
row[2][Z] = row[2][Z] - shear[Z]*row[1][Z];
scale[Z] = vect_mag (row[2]);
vect_normalize (row[2]);
if (scale[Z] != 0.0) {
shear[Y] /= scale[Z];
shear[Z] /= scale[Z];
}
vect_cross (temp, row[1], row[2]);
if (vect_dot (row[0], temp) < 0.0) {
for (i = 0; i < 3; i++) {
scale[i] *= -1.0;
row[i][X] *= -1.0;
row[i][Y] *= -1.0;
row[i][Z] *= -1.0;
}
}
if (row[0][Z] < -1.0) row[0][Z] = -1.0;
if (row[0][Z] > +1.0) row[0][Z] = +1.0;
rotate[Y] = asin(-row[0][Z]);
if (fabs(cos(rotate[Y])) > EPSILON) {
rotate[X] = atan2 (row[1][Z], row[2][Z]);
rotate[Z] = atan2 (row[0][Y], row[0][X]);
}
else {
rotate[X] = atan2 (row[1][X], row[1][Y]);
rotate[Z] = 0.0;
}
/* Convert the rotations to degrees */
rotate[X] = (180.0/PI)*rotate[X];
rotate[Y] = (180.0/PI)*rotate[Y];
rotate[Z] = (180.0/PI)*rotate[Z];
}
int AD_PatchObject::Tassellate_Texture(void)
{
// ***************************************************************
// TASSELLIZZAZIONE con generazione anche delle coordinate texture
// ***************************************************************
AD_Vertex3D *points_already_calculated, *points_to_calculate;
AD_Vertex2D *UVpoints_already_calculated, *UVpoints_to_calculate;
float4 u_step, v_step, u, v;
float4 tu0, tu1, tv0, tv1, dtu, dtv;
int num_u_step, num_v_step, w, i, j;
int ntria=0;
AD_Vect3D p1, p2;
u_step=1.0f/(u_evaluations-1); num_u_step=(int)(u_evaluations);
v_step=1.0f/(v_evaluations-1); num_v_step=(int)(v_evaluations);
points_already_calculated=&vertex3D[0];
points_to_calculate=&vertex3D[num_u_step];
UVpoints_already_calculated=&vertex2D[0];
UVpoints_to_calculate=&vertex2D[num_u_step];
ntria=0;
for (w=0; w<num_patches; w++)
{
int patcul=Is_Patch_Culled(&patches[w]);
if (!patcul)
{
// precalcolo fila iniziale (isoparametrica v=0)
v=u=0;
dtu=(UVverteces[patches[w].UVvert[3]].u-
UVverteces[patches[w].UVvert[0]].u)/(u_evaluations-1);
dtv=(UVverteces[patches[w].UVvert[3]].v-
UVverteces[patches[w].UVvert[0]].v)/(u_evaluations-1);
tu0=UVverteces[patches[w].UVvert[0]].u;
tv0=UVverteces[patches[w].UVvert[0]].v;
for (i=0; i<num_u_step; i++)
{
Evaluate_Patch(&patches[w], u, 0, &points_already_calculated[i].tpoint);
UVpoints_already_calculated[i].u=tu0;
UVpoints_already_calculated[i].v=tv0;
tu0+=dtu;
tv0+=dtv;
u+=u_step;
}
v=0;
for (j=0; j<num_v_step-1; j++)
{
v+=v_step;
u=0;
tu0= v*UVverteces[patches[w].UVvert[1]].u+
(1-v)*UVverteces[patches[w].UVvert[0]].u;
tv0= v*UVverteces[patches[w].UVvert[1]].v+
(1-v)*UVverteces[patches[w].UVvert[0]].v;
tu1= v*UVverteces[patches[w].UVvert[2]].u+
(1-v)*UVverteces[patches[w].UVvert[3]].u;
tv1= v*UVverteces[patches[w].UVvert[2]].v+
(1-v)*UVverteces[patches[w].UVvert[3]].v;
dtu=(tu1-tu0)/(u_evaluations-1);
dtv=(tv1-tv0)/(u_evaluations-1);
for (i=0; i<num_u_step; i++)
{
Evaluate_Patch(&patches[w], u, v, &points_to_calculate[i].tpoint);
UVpoints_to_calculate[i].u=tu0;
UVpoints_to_calculate[i].v=tv0;
tu0+=dtu;
tv0+=dtv;
u+=u_step;
}
// creazione dei triangoli
for (i=0; i<num_u_step-1; i++)
{
tria[ntria].v1=&points_already_calculated[i];
tria[ntria].v2=&points_already_calculated[i+1];
tria[ntria].v3=&points_to_calculate[i+1];
// calcolo della normale
vect_sub_inline(&tria[ntria].v1->tpoint, &tria[ntria].v2->tpoint, &p1);
vect_sub_inline(&tria[ntria].v3->tpoint, &tria[ntria].v2->tpoint, &p2);
vect_cross(&p1, &p2, &tria[ntria].normal);
vect_auto_normalize(&tria[ntria].normal);
tria[ntria].sp1=&UVpoints_already_calculated[i];
tria[ntria].sp2=&UVpoints_already_calculated[i+1];
tria[ntria].sp3=&UVpoints_to_calculate[i+1];
ntria++;
tria[ntria].v1=&points_to_calculate[i+1];
tria[ntria].v2=&points_to_calculate[i];
tria[ntria].v3=&points_already_calculated[i];
// calcolo della normale
vect_sub_inline(&tria[ntria].v1->tpoint, &tria[ntria].v2->tpoint, &p1);
vect_sub_inline(&tria[ntria].v3->tpoint, &tria[ntria].v2->tpoint, &p2);
vect_cross(&p1, &p2, &tria[ntria].normal);
vect_auto_normalize(&tria[ntria].normal);
tria[ntria].sp1=&UVpoints_to_calculate[i+1];
tria[ntria].sp2=&UVpoints_to_calculate[i];
tria[ntria].sp3=&UVpoints_already_calculated[i];
ntria++;
}
points_already_calculated+=num_u_step;
points_to_calculate+=num_u_step;
UVpoints_already_calculated+=num_u_step;
UVpoints_to_calculate+=num_u_step;
}
points_already_calculated+=num_u_step;
points_to_calculate+=num_u_step;
UVpoints_already_calculated+=num_u_step;
UVpoints_to_calculate+=num_u_step;
}
}
return(ntria);
}
/**
* Recursive Newton-Euler algorithm.
*
* @Note the parameter \p stride which is used to allow for input and output
* arrays which are 2-dimensional but in column-major (Matlab) order. We
* need to access rows from the arrays.
*
*/
void
newton_euler (
Robot *robot, /*!< robot object */
double *tau, /*!< returned joint torques */
double *qd, /*!< joint velocities */
double *qdd, /*!< joint accelerations */
double *fext, /*!< external force on manipulator tip */
int stride /*!< indexing stride for qd, qdd */
) {
Vect t1, t2, t3, t4;
Vect qdv, qddv;
Vect F, N;
Vect z0 = {0.0, 0.0, 1.0};
Vect zero = {0.0, 0.0, 0.0};
Vect f_tip = {0.0, 0.0, 0.0};
Vect n_tip = {0.0, 0.0, 0.0};
register int j;
double t;
Link *links = robot->links;
/*
* angular rate and acceleration vectors only have finite
* z-axis component
*/
qdv = qddv = zero;
/* setup external force/moment vectors */
if (fext) {
f_tip.x = fext[0];
f_tip.y = fext[1];
f_tip.z = fext[2];
n_tip.x = fext[3];
n_tip.y = fext[4];
n_tip.z = fext[5];
}
/******************************************************************************
* forward recursion --the kinematics
******************************************************************************/
if (robot->dhtype == MODIFIED) {
/*
* MODIFIED D&H CONVENTIONS
*/
for (j = 0; j < robot->njoints; j++) {
/* create angular vector from scalar input */
qdv.z = qd[j*stride];
qddv.z = qdd[j*stride];
switch (links[j].sigma) {
case REVOLUTE:
/*
* calculate angular velocity of link j
*/
if (j == 0)
*OMEGA(j) = qdv;
else {
rot_trans_vect_mult (&t1, ROT(j), OMEGA(j-1));
vect_add (OMEGA(j), &t1, &qdv);
}
/*
* calculate angular acceleration of link j
*/
if (j == 0)
*OMEGADOT(j) = qddv;
else {
rot_trans_vect_mult (&t3, ROT(j), OMEGADOT(j-1));
vect_cross (&t2, &t1, &qdv);
vect_add (&t1, &t2, &t3);
vect_add (OMEGADOT(j), &t1, &qddv);
}
/*
* compute acc[j]
*/
if (j == 0) {
t1 = *robot->gravity;
} else {
vect_cross(&t1, OMEGA(j-1), PSTAR(j));
vect_cross(&t2, OMEGA(j-1), &t1);
vect_cross(&t1, OMEGADOT(j-1), PSTAR(j));
vect_add(&t1, &t1, &t2);
vect_add(&t1, &t1, ACC(j-1));
}
rot_trans_vect_mult(ACC(j), ROT(j), &t1);
break;
case PRISMATIC:
/*
* calculate omega[j]
*/
if (j == 0)
*(OMEGA(j)) = qdv;
else
rot_trans_vect_mult (OMEGA(j), ROT(j), OMEGA(j-1));
/*
* calculate alpha[j]
*/
if (j == 0)
*(OMEGADOT(j)) = qddv;
else
rot_trans_vect_mult (OMEGADOT(j), ROT(j), OMEGADOT(j-1));
/*
* compute acc[j]
*/
if (j == 0) {
*ACC(j) = *robot->gravity;
} else {
vect_cross(&t1, OMEGADOT(j-1), PSTAR(j));
vect_cross(&t3, OMEGA(j-1), PSTAR(j));
vect_cross(&t2, OMEGA(j-1), &t3);
vect_add(&t1, &t1, &t2);
vect_add(&t1, &t1, ACC(j-1));
rot_trans_vect_mult(ACC(j), ROT(j), &t1);
rot_trans_vect_mult(&t2, ROT(j), OMEGA(j-1));
vect_cross(&t1, &t2, &qdv);
scal_mult(&t1, &t1, 2.0);
vect_add(ACC(j), ACC(j), &t1);
vect_add(ACC(j), ACC(j), &qddv);
}
break;
}
/*
* compute abar[j]
*/
vect_cross(&t1, OMEGADOT(j), R_COG(j));
vect_cross(&t2, OMEGA(j), R_COG(j));
vect_cross(&t3, OMEGA(j), &t2);
vect_add(ACC_COG(j), &t1, &t3);
vect_add(ACC_COG(j), ACC_COG(j), ACC(j));
#ifdef DEBUG
vect_print("w", OMEGA(j));
vect_print("wd", OMEGADOT(j));
vect_print("acc", ACC(j));
vect_print("abar", ACC_COG(j));
#endif
}
} else {
/*
* STANDARD D&H CONVENTIONS
*/
for (j = 0; j < robot->njoints; j++) {
/* create angular vector from scalar input */
qdv.z = qd[j*stride];
qddv.z = qdd[j*stride];
switch (links[j].sigma) {
case REVOLUTE:
/*
* calculate omega[j]
*/
if (j == 0)
t1 = qdv;
else
vect_add (&t1, OMEGA(j-1), &qdv);
rot_trans_vect_mult (OMEGA(j), ROT(j), &t1);
/*
* calculate alpha[j]
*/
if (j == 0)
t3 = qddv;
else {
vect_add (&t1, OMEGADOT(j-1), &qddv);
vect_cross (&t2, OMEGA(j-1), &qdv);
vect_add (&t3, &t1, &t2);
}
rot_trans_vect_mult (OMEGADOT(j), ROT(j), &t3);
/*
* compute acc[j]
*/
vect_cross(&t1, OMEGADOT(j), PSTAR(j));
vect_cross(&t2, OMEGA(j), PSTAR(j));
vect_cross(&t3, OMEGA(j), &t2);
vect_add(ACC(j), &t1, &t3);
if (j == 0) {
rot_trans_vect_mult(&t1, ROT(j), robot->gravity);
} else
rot_trans_vect_mult(&t1, ROT(j), ACC(j-1));
vect_add(ACC(j), ACC(j), &t1);
break;
case PRISMATIC:
/*
* calculate omega[j]
*/
if (j == 0)
*(OMEGA(j)) = zero;
else
rot_trans_vect_mult (OMEGA(j), ROT(j), OMEGA(j-1));
/*
* calculate alpha[j]
*/
if (j == 0)
*(OMEGADOT(j)) = zero;
else
rot_trans_vect_mult (OMEGADOT(j), ROT(j), OMEGADOT(j-1));
/*
* compute acc[j]
*/
if (j == 0) {
vect_add(&qddv, &qddv, robot->gravity);
rot_trans_vect_mult(ACC(j), ROT(j), &qddv);
} else {
vect_add(&t1, &qddv, ACC(j-1));
rot_trans_vect_mult(ACC(j), ROT(j), &t1);
}
vect_cross(&t1, OMEGADOT(j), PSTAR(j));
vect_add(ACC(j), ACC(j), &t1);
rot_trans_vect_mult(&t1, ROT(j), &qdv);
vect_cross(&t2, OMEGA(j), &t1);
scal_mult(&t2, &t2, 2.0);
vect_add(ACC(j), ACC(j), &t2);
vect_cross(&t2, OMEGA(j), PSTAR(j));
vect_cross(&t3, OMEGA(j), &t2);
vect_add(ACC(j), ACC(j), &t3);
break;
}
/*
* compute abar[j]
*/
vect_cross(&t1, OMEGADOT(j), R_COG(j));
vect_cross(&t2, OMEGA(j), R_COG(j));
vect_cross(&t3, OMEGA(j), &t2);
vect_add(ACC_COG(j), &t1, &t3);
vect_add(ACC_COG(j), ACC_COG(j), ACC(j));
#ifdef DEBUG
vect_print("w", OMEGA(j));
vect_print("wd", OMEGADOT(j));
vect_print("acc", ACC(j));
vect_print("abar", ACC_COG(j));
#endif
}
}
/******************************************************************************
* backward recursion part --the kinetics
******************************************************************************/
if (robot->dhtype == MODIFIED) {
/*
* MODIFIED D&H CONVENTIONS
*/
for (j = robot->njoints - 1; j >= 0; j--) {
/*
* compute F[j]
*/
scal_mult (&F, ACC_COG(j), M(j));
/*
* compute f[j]
*/
if (j == (robot->njoints-1))
t1 = f_tip;
else
rot_vect_mult (&t1, ROT(j+1), f(j+1));
vect_add (f(j), &t1, &F);
/*
* compute N[j]
*/
mat_vect_mult(&t2, INERTIA(j), OMEGADOT(j));
mat_vect_mult(&t3, INERTIA(j), OMEGA(j));
vect_cross(&t4, OMEGA(j), &t3);
vect_add(&N, &t2, &t4);
/*
* compute n[j]
*/
if (j == (robot->njoints-1))
t1 = n_tip;
else {
rot_vect_mult(&t1, ROT(j+1), n(j+1));
rot_vect_mult(&t4, ROT(j+1), f(j+1));
vect_cross(&t3, PSTAR(j+1), &t4);
vect_add(&t1, &t1, &t3);
}
vect_cross(&t2, R_COG(j), &F);
vect_add(&t1, &t1, &t2);
vect_add(n(j), &t1, &N);
#ifdef DEBUG
vect_print("f", f(j));
vect_print("n", n(j));
#endif
}
} else {
/*
* STANDARD D&H CONVENTIONS
*/
for (j = robot->njoints - 1; j >= 0; j--) {
/*
* compute f[j]
*/
scal_mult (&t4, ACC_COG(j), M(j));
if (j != (robot->njoints-1)) {
rot_vect_mult (&t1, ROT(j+1), f(j+1));
vect_add (f(j), &t4, &t1);
} else
vect_add (f(j), &t4, &f_tip);
/*
* compute n[j]
*/
/* cross(pstar+r,Fm(:,j)) */
vect_add(&t2, PSTAR(j), R_COG(j));
vect_cross(&t1, &t2, &t4);
if (j != (robot->njoints-1)) {
/* cross(R'*pstar,f) */
rot_trans_vect_mult(&t2, ROT(j+1), PSTAR(j));
vect_cross(&t3, &t2, f(j+1));
/* nn += R*(nn + cross(R'*pstar,f)) */
vect_add(&t3, &t3, n(j+1));
rot_vect_mult(&t2, ROT(j+1), &t3);
vect_add(&t1, &t1, &t2);
} else {
/* cross(R'*pstar,f) */
vect_cross(&t2, PSTAR(j), &f_tip);
/* nn += R*(nn + cross(R'*pstar,f)) */
vect_add(&t1, &t1, &t2);
vect_add(&t1, &t1, &n_tip);
}
mat_vect_mult(&t2, INERTIA(j), OMEGADOT(j));
mat_vect_mult(&t3, INERTIA(j), OMEGA(j));
vect_cross(&t4, OMEGA(j), &t3);
vect_add(&t2, &t2, &t4);
vect_add(n(j), &t1, &t2);
#ifdef DEBUG
vect_print("f", f(j));
vect_print("n", n(j));
#endif
}
}
/*
* Compute the torque total for each axis
*
*/
for (j=0; j < robot->njoints; j++) {
double t;
Link *l = &links[j];
if (robot->dhtype == MODIFIED)
t1 = z0;
else
rot_trans_vect_mult(&t1, ROT(j), &z0);
switch (l->sigma) {
case REVOLUTE:
t = vect_dot(n(j), &t1);
break;
case PRISMATIC:
t = vect_dot(f(j), &t1);
break;
}
/*
* add actuator dynamics and friction
*/
t += l->G * l->G * l->Jm * qdd[j*stride]; // inertia
t += l->G * l->G * l->B * qd[j*stride]; // viscous friction
t += fabs(l->G) * (
(qd[j*stride] > 0 ? l->Tc[0] : 0.0) + // Coulomb friction
(qd[j*stride] < 0 ? l->Tc[1] : 0.0)
);
tau[j*stride] = t;
}
}
