static int
test_bn_mat_dup(void)
{
mat_t m = {1.1, 2.2, 3.3, 4.4, 5.5, 6.6, 7.7, 8.8, 9.9, 10.1, 11.11, 12.12, 13.13, 14.14, 15.15, 16.16};
matp_t expected = m, actual;
actual = bn_mat_dup(m);
return !mat_equal(expected, actual);
}
static int
test_bn_mat_trn(int argc, char *argv[])
{
mat_t m, expected, actual;
if (argc != 4) {
bu_exit(1, "<args> format: M <expected_result> [%s]\n", argv[0]);
}
scan_mat_args(argv, 2, &m);
scan_mat_args(argv, 3, &expected);
bn_mat_trn(actual, m);
return !mat_equal(expected, actual);
}
static int
test_bn_mat_mul(int argc, char *argv[])
{
mat_t m1, m2, expected, actual;
if (argc != 5) {
bu_exit(1, "<args> format: M1 M2 <expected_result> [%s]\n", argv[0]);
}
scan_mat_args(argv, 2, &m1);
scan_mat_args(argv, 3, &m2);
scan_mat_args(argv, 4, &expected);
bn_mat_mul(actual, m1, m2);
return !mat_equal(expected, actual);
}
static int
test_bn_mat_xform_about_pt(int argc, char *argv[])
{
mat_t xform, expected, actual;
point_t p;
if (argc != 5) {
bu_exit(1, "<args> format: M P <expected_result> [%s]\n", argv[0]);
}
scan_mat_args(argv, 2, &xform);
sscanf(argv[3], "%lg,%lg,%lg", &p[0], &p[1], &p[2]);
scan_mat_args(argv, 4, &expected);
bn_mat_xform_about_pt(actual, xform, p);
return !mat_equal(expected, actual);
}
static int
test_bn_mat_zrot(int argc, char *argv[])
{
mat_t expected, actual;
double sinz, cosz;
if (argc != 5) {
bu_exit(1, "<args> format: sinz cosz <expected_result> [%s]\n", argv[0]);
}
sscanf(argv[2], "%lg", &sinz);
sscanf(argv[3], "%lg", &cosz);
scan_mat_args(argv, 4, &expected);
bn_mat_zrot(actual, sinz, cosz);
return !mat_equal(expected, actual);
}
static int
test_bn_mat_ae(int argc, char *argv[])
{
double az, el;
mat_t expected, actual;
if (argc != 5) {
bu_exit(1, "<args> format: az el <expected_result> [%s]\n", argv[0]);
}
sscanf(argv[2], "%lg", &az);
sscanf(argv[3], "%lg", &el);
scan_mat_args(argv, 4, &expected);
bn_mat_ae(actual, az, el);
return !mat_equal(expected, actual);
}
static int
test_bn_mat_angles_rad(int argc, char *argv[])
{
mat_t expected, actual;
double x, y, z;
if (argc != 6) {
bu_exit(1, "<args> format: x y z <expected_result> [%s]\n", argv[0]);
}
sscanf(argv[2], "%lg", &x);
sscanf(argv[3], "%lg", &y);
sscanf(argv[4], "%lg", &z);
scan_mat_args(argv, 5, &expected);
bn_mat_angles_rad(actual, x, y, z);
return !mat_equal(expected, actual);
}
static int
test_bn_mat_scale_about_pt(int argc, char *argv[])
{
mat_t expected, actual;
point_t p;
double s;
int error, expected_error;
if (argc != 6) {
bu_exit(1, "<args> format: P s error <expected_result> [%s]\n", argv[0]);
}
sscanf(argv[2], "%lg,%lg,%lg", &p[0], &p[1], &p[2]);
sscanf(argv[3], "%lg", &s);
sscanf(argv[4], "%d", &expected_error);
scan_mat_args(argv, 5, &expected);
error = bn_mat_scale_about_pt(actual, p, s);
return !(mat_equal(expected, actual) && error == expected_error);
}
static int
test_bn_mat_inverse(int argc, char *argv[])
{
mat_t m, expected, actual;
int singular;
sscanf(argv[2], "%d", &singular);
if ((argc == 4 && !singular) || (argc != 4 && argc != 5)) {
bu_exit(1, "<args> format: 0|1 M [expected_result] [%s]\n", argv[0]);
}
scan_mat_args(argv, 3, &m);
if (!singular) {
scan_mat_args(argv, 4, &expected);
}
if (!bn_mat_inverse(actual, m)) {
return !singular;
}
return !mat_equal(expected, actual);
}
bool
operator==(mat<N,S> const& a, mat<N,S> const& b) {
return mat_equal(a, b);
}
int
main(void)
{
int i, result;
flint_rand_t state;
printf("inv... ");
fflush(stdout);
_randinit(state);
/* Check aliasing */
/* Managed element type (mpq_t) */
for (i = 0; i < 50; i++)
{
long n;
ctx_t ctx;
mat_t A, B, C;
long ansB, ansC;
n = n_randint(state, 20) + 1;
ctx_init_mpq(ctx);
mat_init(A, n, n, ctx);
mat_init(B, n, n, ctx);
mat_init(C, n, n, ctx);
mat_randtest(A, state, ctx);
mat_set(B, A, ctx);
ansC = mat_inv(C, B, ctx);
ansB = mat_inv(B, B, ctx);
result = ((ansB == ansC) && (!ansB || mat_equal(B, C, ctx)));
if (!result)
{
printf("FAIL:\n\n");
printf("A: \n"), mat_print(A, ctx), printf("\n");
printf("B: \n"), mat_print(B, ctx), printf("\n");
printf("C: \n"), mat_print(C, ctx), printf("\n");
printf("ansB = %ld\n", ansB);
printf("ansC = %ld\n", ansC);
}
mat_clear(A, ctx);
mat_clear(B, ctx);
mat_clear(C, ctx);
ctx_clear(ctx);
}
/* Check A * A^{-1} == A^{-1} * A == Id */
/* Managed element type (mpq_t) */
for (i = 0; i < 50; i++)
{
long n;
ctx_t ctx;
mat_t A, B, C, D, I;
long ansB;
n = n_randint(state, 20) + 1;
ctx_init_mpq(ctx);
mat_init(A, n, n, ctx);
mat_init(B, n, n, ctx);
mat_init(C, n, n, ctx);
mat_init(D, n, n, ctx);
mat_init(I, n, n, ctx);
mat_randtest(A, state, ctx);
ansB = mat_inv(B, A, ctx);
if (!ansB)
{
mat_mul(C, A, B, ctx);
mat_mul(D, B, A, ctx);
}
result = (ansB || (mat_equal(C, D, ctx) && mat_is_one(C, ctx)));
if (!result)
{
printf("FAIL:\n\n");
printf("A: \n"), mat_print(A, ctx), printf("\n");
printf("B: \n"), mat_print(B, ctx), printf("\n");
printf("C: \n"), mat_print(C, ctx), printf("\n");
printf("D: \n"), mat_print(D, ctx), printf("\n");
printf("ansB = %ld\n", ansB);
}
mat_clear(A, ctx);
mat_clear(B, ctx);
mat_clear(C, ctx);
mat_clear(D, ctx);
ctx_clear(ctx);
}
_randclear(state);
_fmpz_cleanup();
printf("PASS\n");
return EXIT_SUCCESS;
}
/*!*****************************************************************************
*******************************************************************************
\note compute_kinematics
\date June 99
\remarks
computes kinematic variables
*******************************************************************************
Function Parameters: [in]=input,[out]=output
none
******************************************************************************/
static void
compute_kinematics(void)
{
int i,j,r,o;
static int firsttime = TRUE;
static SL_DJstate *temp;
static Matrix last_J;
static Matrix last_Jbase;
if (firsttime) {
temp=(SL_DJstate *)my_calloc((unsigned long)(n_dofs+1),sizeof(SL_DJstate),MY_STOP);
last_J = my_matrix(1,6*n_endeffs,1,n_dofs);
last_Jbase = my_matrix(1,6*n_endeffs,1,2*N_CART);
}
/* compute the desired link positions */
linkInformationDes(joint_des_state,&base_state,&base_orient,endeff,
joint_cog_mpos_des,joint_axis_pos_des,joint_origin_pos_des,
link_pos_des,Alink_des,Adof_des);
/* the desired endeffector information */
for (i=1; i<=N_CART; ++i) {
for (j=1; j<=n_endeffs; ++j) {
cart_des_state[j].x[i] = link_pos_des[link2endeffmap[j]][i];
}
}
/* the desired quaternian of the endeffector */
for (j=1; j<=n_endeffs; ++j) {
linkQuat(Alink_des[link2endeffmap[j]],&(cart_des_orient[j]));
}
/* addititional link information */
linkInformation(joint_state,&base_state,&base_orient,endeff,
joint_cog_mpos,joint_axis_pos,joint_origin_pos,
link_pos,Alink,Adof);
/* create the endeffector information */
for (i=1; i<=N_CART; ++i) {
for (j=1; j<=n_endeffs; ++j) {
cart_state[j].x[i] = link_pos[link2endeffmap[j]][i];
}
}
/* the quaternian of the endeffector */
for (j=1; j<=n_endeffs; ++j) {
linkQuat(Alink[link2endeffmap[j]],&(cart_orient[j]));
}
/* the COG position */
compute_cog();
/* the jacobian */
jacobian(link_pos,joint_origin_pos,joint_axis_pos,J);
baseJacobian(link_pos,joint_origin_pos,joint_axis_pos,Jbase);
jacobian(link_pos_des,joint_origin_pos_des,joint_axis_pos_des,Jdes);
baseJacobian(link_pos_des,joint_origin_pos_des,joint_axis_pos_des,Jbasedes);
/* numerical time derivative of Jacobian */
if (!firsttime) {
mat_sub(J,last_J,dJdt);
mat_mult_scalar(dJdt,(double)ros_servo_rate,dJdt);
mat_sub(Jbase,last_Jbase,dJbasedt);
mat_mult_scalar(dJbasedt,(double)ros_servo_rate,dJbasedt);
}
mat_equal(J,last_J);
mat_equal(Jbase,last_Jbase);
/* compute the cartesian velocities and accelerations */
for (j=1; j<=n_endeffs; ++j) {
for (i=1; i<=N_CART; ++i) {
cart_state[j].xd[i] = 0.0;
cart_state[j].xdd[i] = 0.0;
cart_orient[j].ad[i] = 0.0;
cart_orient[j].add[i] = 0.0;
cart_des_state[j].xd[i] = 0.0;
cart_des_orient[j].ad[i] = 0.0;
/* contributations from the joints */
for (r=1; r<=n_dofs; ++r) {
cart_state[j].xd[i] += J[(j-1)*6+i][r] * joint_state[r].thd;
cart_orient[j].ad[i] += J[(j-1)*6+i+3][r] * joint_state[r].thd;
cart_des_state[j].xd[i] += Jdes[(j-1)*6+i][r] *joint_des_state[r].thd;
cart_des_orient[j].ad[i]+= Jdes[(j-1)*6+i+3][r] * joint_des_state[r].thd;
cart_state[j].xdd[i] += J[(j-1)*6+i][r] * joint_state[r].thdd +
dJdt[(j-1)*6+i][r] * joint_state[r].thd;
cart_orient[j].add[i] += J[(j-1)*6+i+3][r] * joint_state[r].thdd +
dJdt[(j-1)*6+i+3][r] * joint_state[r].thd;
}
/* contributations from the base */
for (r=1; r<=N_CART; ++r) {
cart_state[j].xd[i] += Jbase[(j-1)*6+i][r] * base_state.xd[r];
cart_orient[j].ad[i] += Jbase[(j-1)*6+i+3][r] * base_state.xd[r];
cart_state[j].xd[i] += Jbase[(j-1)*6+i][3+r] * base_orient.ad[r];
cart_orient[j].ad[i] += Jbase[(j-1)*6+i+3][3+r] * base_orient.ad[r];
cart_des_state[j].xd[i] += Jbasedes[(j-1)*6+i][r] * base_state.xd[r];
cart_des_orient[j].ad[i] += Jbasedes[(j-1)*6+i+3][r] * base_state.xd[r];
cart_des_state[j].xd[i] += Jbasedes[(j-1)*6+i][3+r] * base_orient.ad[r];
cart_des_orient[j].ad[i] += Jbasedes[(j-1)*6+i+3][3+r] * base_orient.ad[r];
cart_state[j].xdd[i] += Jbase[(j-1)*6+i][r] * base_state.xdd[r] +
dJbasedt[(j-1)*6+i][r] * base_state.xd[r];
cart_orient[j].add[i] += Jbase[(j-1)*6+i+3][r] * base_state.xdd[r] +
dJbasedt[(j-1)*6+i+3][r] * base_state.xd[r];
cart_state[j].xdd[i] += Jbase[(j-1)*6+i][3+r] * base_orient.add[r] +
dJbasedt[(j-1)*6+i][3+r] * base_orient.ad[r];
cart_orient[j].add[i] += Jbase[(j-1)*6+i+3][3+r] * base_orient.add[r] +
dJbasedt[(j-1)*6+i+3][3+r] * base_orient.ad[r];
}
}
/* compute quaternion derivatives */
quatDerivatives(&(cart_orient[j]));
quatDerivatives(&(cart_des_orient[j]));
for (r=1; r<=N_QUAT; ++r)
cart_des_orient[j].qdd[r] = 0.0; // we don't have dJdes_dt so far
}
/* reset first time flag */
firsttime = FALSE;
}
int
main(void)
{
int i, result;
flint_rand_t state;
printf("add... ");
fflush(stdout);
_randinit(state);
/* Check that addition is abelian */
/* Unmanaged element type (long) */
for (i = 0; i < 100; i++)
{
long m, n;
ctx_t ctx;
mat_t A, B, C, D;
m = n_randint(state, 50) + 1;
n = n_randint(state, 50) + 1;
ctx_init_long(ctx);
mat_init(A, m, n, ctx);
mat_init(B, m, n, ctx);
mat_init(C, m, n, ctx);
mat_init(D, m, n, ctx);
mat_randtest(A, state, ctx);
mat_randtest(B, state, ctx);
mat_add(C, A, B, ctx);
mat_add(D, B, A, ctx);
result = mat_equal(C, D, ctx);
if (!result)
{
printf("FAIL:\n\n");
printf("Matrix A\n");
mat_print(A, ctx);
printf("\n");
printf("Matrix A\n");
mat_print(B, ctx);
printf("\n");
}
mat_clear(A, ctx);
mat_clear(B, ctx);
mat_clear(C, ctx);
mat_clear(D, ctx);
ctx_clear(ctx);
}
/* Managed element type (mpq_t) */
for (i = 0; i < 100; i++)
{
long m, n;
ctx_t ctx;
mat_t A, B, C, D;
m = n_randint(state, 50) + 1;
n = n_randint(state, 50) + 1;
ctx_init_mpq(ctx);
mat_init(A, m, n, ctx);
mat_init(B, m, n, ctx);
mat_init(C, m, n, ctx);
mat_init(D, m, n, ctx);
mat_randtest(A, state, ctx);
mat_randtest(B, state, ctx);
mat_add(C, A, B, ctx);
mat_add(D, B, A, ctx);
result = mat_equal(C, D, ctx);
if (!result)
{
printf("FAIL:\n\n");
printf("Matrix A\n");
mat_print(A, ctx);
printf("\n");
printf("Matrix A\n");
mat_print(B, ctx);
printf("\n");
}
mat_clear(A, ctx);
mat_clear(B, ctx);
mat_clear(C, ctx);
mat_clear(D, ctx);
ctx_clear(ctx);
}
/* Check that the zero matrix does what it's supposed to do */
/* Unmanaged element type (long) */
for (i = 0; i < 100; i++)
{
long m, n;
ctx_t ctx;
mat_t A, B, C, D;
m = n_randint(state, 50) + 1;
n = n_randint(state, 50) + 1;
ctx_init_long(ctx);
mat_init(A, m, n, ctx);
mat_init(B, m, n, ctx);
mat_init(C, m, n, ctx);
mat_init(D, m, n, ctx);
mat_randtest(A, state, ctx);
mat_zero(B, ctx);
mat_add(C, A, B, ctx);
mat_add(D, B, A, ctx);
result = mat_equal(A, C, ctx) && mat_equal(C, D, ctx);
if (!result)
{
printf("FAIL:\n\n");
printf("Matrix A\n");
mat_print(A, ctx);
printf("\n");
printf("Matrix A\n");
mat_print(B, ctx);
printf("\n");
}
mat_clear(A, ctx);
mat_clear(B, ctx);
mat_clear(C, ctx);
mat_clear(D, ctx);
ctx_clear(ctx);
}
/* Managed element type (mpq_t) */
for (i = 0; i < 100; i++)
{
long m, n;
ctx_t ctx;
mat_t A, B, C, D;
m = n_randint(state, 50) + 1;
n = n_randint(state, 50) + 1;
ctx_init_mpq(ctx);
mat_init(A, m, n, ctx);
mat_init(B, m, n, ctx);
mat_init(C, m, n, ctx);
mat_init(D, m, n, ctx);
mat_randtest(A, state, ctx);
mat_zero(B, ctx);
mat_add(C, A, B, ctx);
mat_add(D, B, A, ctx);
result = mat_equal(A, C, ctx) && mat_equal(C, D, ctx);
if (!result)
{
printf("FAIL:\n\n");
printf("Matrix A\n");
mat_print(A, ctx);
printf("\n");
printf("Matrix A\n");
mat_print(B, ctx);
printf("\n");
}
mat_clear(A, ctx);
mat_clear(B, ctx);
mat_clear(C, ctx);
mat_clear(D, ctx);
ctx_clear(ctx);
}
_randclear(state);
_fmpz_cleanup();
printf("PASS\n");
return EXIT_SUCCESS;
}
void expm(Matrix A, Matrix B, double h) {
static int firsttime = TRUE;
static Matrix M;
static Matrix M2;
static Matrix M3;
static Matrix D;
static Matrix N;
if (firsttime) {
firsttime = FALSE;
M = my_matrix(1,3*N_DOFS,1,3*N_DOFS);
M2 = my_matrix(1,3*N_DOFS,1,3*N_DOFS);
M3 = my_matrix(1,3*N_DOFS,1,3*N_DOFS);
D = my_matrix(1,3*N_DOFS,1,3*N_DOFS);
N = my_matrix(1,3*N_DOFS,1,3*N_DOFS);
}
int norm = 0;
double c = 0.5;
int q = 6; // uneven p-q order for Pade approx.
int p = 1;
int i,j,k;
// Form the big matrix
mat_mult_scalar(A, h, A);
mat_equal_size(A, 2*N_DOFS, 2*N_DOFS, M);
for (i = 1; i <= 2*N_DOFS; ++i) {
for (j = 1; j <= N_DOFS; ++j) {
M[i][2*N_DOFS + j] = h * B[i][j];
}
}
//print_mat("M is:\n", M);
// scale M by power of 2 so that its norm < 1/2
norm = (int)(log2(inf_norm(M))) + 2;
//printf("Inf norm of M: %f \n", inf_norm(M));
if (norm < 0)
norm = 0;
mat_mult_scalar(M, 1/pow(2,(double)norm), M);
//printf("Norm of M logged, floored and 2 added is: %d\n", norm);
//print_mat("M after scaling is:\n", M);
for (i = 1; i <= 3*N_DOFS; i++) {
for (j = 1; j <= 3*N_DOFS; j++) {
N[i][j] = c * M[i][j];
D[i][j] = -c * M[i][j];
}
N[i][i] = N[i][i] + 1.0;
D[i][i] = D[i][i] + 1.0;
}
// set M2 equal to M
mat_equal(M, M2);
// start pade approximation
for (k = 2; k <= q; k++) {
c = c * (q - k + 1) / (double)(k * (2*q - k + 1));
mat_mult(M,M2,M2);
mat_mult_scalar(M2,c,M3);
mat_add(N,M3,N);
if (p == 1)
mat_add(D,M3,D);
else
mat_sub(D,M3,D);
p = -1 * p;
}
// multiply denominator with nominator i.e. D\E
my_inv_ludcmp(D, 3*N_DOFS, D);
mat_mult(D,N,N);
// undo scaling by repeated squaring
for (k = 1; k <= norm; k++)
mat_mult(N,N,N);
// get the matrices out
// read off the entries
for (i = 1; i <= 2*N_DOFS; i++) {
for (j = 1; j <= 2*N_DOFS; j++) {
A[i][j] = N[i][j];
}
for (j = 1; j <= N_DOFS; j++) {
B[i][j] = N[i][2*N_DOFS + j];
}
}
}
int
main(void)
{
int i, result;
flint_rand_t state;
printf("transpose... ");
fflush(stdout);
_randinit(state);
/* Check aliasing */
/* Unmanaged element type (long) */
for (i = 0; i < 100; i++)
{
long m, n;
ctx_t ctx;
mat_t A, B, C;
m = n_randint(state, 50) + 1;
n = m;
ctx_init_long(ctx);
mat_init(A, m, n, ctx);
mat_init(B, m, n, ctx);
mat_init(C, m, n, ctx);
mat_randtest(A, state, ctx);
mat_set(B, A, ctx);
mat_transpose(C, B, ctx);
mat_transpose(B, B, ctx);
result = mat_equal(B, C, ctx);
if (!result)
{
printf("FAIL:\n\n");
printf("Matrix A\n");
mat_print(A, ctx);
printf("\n");
printf("Matrix B\n");
mat_print(B, ctx);
printf("\n");
printf("Matrix C\n");
mat_print(C, ctx);
printf("\n");
}
mat_clear(A, ctx);
mat_clear(B, ctx);
mat_clear(C, ctx);
ctx_clear(ctx);
}
/* Check that (A^t)^t == A */
/* Unmanaged element type (long) */
for (i = 0; i < 100; i++)
{
long m, n;
ctx_t ctx;
mat_t A, B, C;
m = n_randint(state, 50) + 1;
n = n_randint(state, 50) + 1;
ctx_init_long(ctx);
mat_init(A, m, n, ctx);
mat_init(B, n, m, ctx);
mat_init(C, m, n, ctx);
mat_randtest(A, state, ctx);
mat_transpose(B, A, ctx);
mat_transpose(C, B, ctx);
result = mat_equal(A, C, ctx);
if (!result)
{
printf("FAIL:\n\n");
printf("Matrix A\n");
mat_print(A, ctx);
printf("\n");
printf("Matrix B\n");
mat_print(B, ctx);
printf("\n");
printf("Matrix C\n");
mat_print(C, ctx);
printf("\n");
}
mat_clear(A, ctx);
mat_clear(B, ctx);
mat_clear(C, ctx);
ctx_clear(ctx);
}
_randclear(state);
_fmpz_cleanup();
printf("PASS\n");
return EXIT_SUCCESS;
}