int
McGripGrasp::computeQuasistaticForces(double tendonForce)
{
//routing matrices
Matrix *B, *a;
static_cast<McGrip *>(hand)->getRoutingMatrices(&B, &a);
//parameter matrix
Matrix p(8, 1);
//tendon insertion points
p.elem(0, 0) = 5;
p.elem(1, 0) = 5;
p.elem(2, 0) = 1.65;
//--------------
p.elem(3, 0) = 5;
p.elem(4, 0) = 5;
p.elem(5, 0) = 1.65;
//link length and joint radius
p.elem(6, 0) = static_cast<McGrip *>(hand)->getJointRadius();
p.elem(7, 0) = static_cast<McGrip *>(hand)->getLinkLength();
//compute joint torques
Matrix tau(6, 1);
matrixMultiply(*B, p, tau);
matrixAdd(tau, *a, tau);
delete a;
delete B;
//multiply by tendon force
tau.multiply(tendonForce);
//call super
return Grasp::computeQuasistaticForces(tau);
}
int main () {
int rows = 2;
int cols = 3;
int matrix1[rows][cols] = {{0,1,2}, {9,8,7}};
int matrix2[rows][cols] = {{6,5,4},{3,4,5}};
int resultMatrix[rows][cols];
printf("Matrix 1:\n");
printMatrix(rows, cols, matrix1);
printf("\nMatrix 2:\n");
printMatrix(rows, cols, matrix2);
printf("\nMatrix 1 + Matrix 2:\n");
matrixAdd(rows, cols, matrix1, matrix2, resultMatrix);
printMatrix(rows, cols, resultMatrix);
printf("\nMatrix 1 - Matrix 2:\n");
matrixSubtract(rows, cols, matrix1, matrix2, resultMatrix);
printMatrix(rows, cols, resultMatrix);
return 0;
}
int main()
{
printf("Testing work:\n");
int n = 9;
int m = 1;
Matrix a = matrixNew(n, m);
for (int i = 0; i < matrixGetRows(a); i++)
for (int j = 0; j < matrixGetCols(a); j++)
matrixSet(a, i, j, (float)(i + 1) / (j + 1));
for (int i = 0; i < matrixGetRows(a); i++)
{
for (int j = 0; j < matrixGetCols(a); j++)
printf("%.5f ", matrixGet(a, i, j));
printf("\n");
}
printf("\n");
Matrix b = matrixScale(a, 10);
for (int i = 0; i < matrixGetRows(b); i++)
{
for (int j = 0; j < matrixGetCols(b); j++)
printf("%.5f ", matrixGet(b, i, j));
printf("\n");
}
printf("\n");
Matrix c = matrixAdd(a, b);
for (int i = 0; i < matrixGetRows(c); i++)
{
for (int j = 0; j < matrixGetCols(c); j++)
printf("%.5f ", matrixGet(c, i, j));
printf("\n");
}
printf("\n");
Matrix d = matrixTranspose(c);
for (int i = 0; i < matrixGetRows(d); i++)
{
for (int j = 0; j < matrixGetCols(d); j++)
printf("%.5f ", matrixGet(d, i, j));
printf("\n");
}
/*c = matrixAdd(a, matrixNew(1, 1));
if (c == NULL)
printf("Yeah\n"), c = matrixNew(3, 3);*/
Matrix e = matrixMul(c, d);
printf("%i %i\n", matrixGetRows(e), matrixGetCols(e));
for (int i = 0; i < matrixGetRows(e); i++)
{
for (int j = 0; j < matrixGetCols(e); j++)
printf("%.5f ", matrixGet(e, i, j));
printf("\n");
}
matrixDelete(a);
matrixDelete(b);
matrixDelete(c);
matrixDelete(d);
matrixDelete(e);
return 0;
}
void convexSpaceCenterPos(struct convexSpace *space, struct convexFigList *vertices) {
int vertCount=0;
matrixZero(space->pos, dim);
if (!vertices)
return;
while (vertices) {
matrixAdd(space->pos, vertices->fig->space->pos, dim);
vertCount++;
vertices=vertices->next;
}
matrixScale(space->pos, 1.0/vertCount, dim);
}
/* Main method to test math */
int main() {
// Test matrix math
struct Matrix a;
//double valuesa[9] = { 5.0, -2.0, 1.0, 0.0, 3.0, -1.0, 2.0, 0.0, 7.0 };
double valuesa[16] = { 1.0, 3.0, -2.0, 1.0, 5.0, 1.0, 0.0, -1.0, 0.0, 1.0, 0.0, -2.0, 2.0, -1.0, 0.0, 3.0 };
newMatrix(&a, valuesa, 4, 4);
struct Matrix b;
double valuesb[9] = { 1.0, 2.0, 3.0, 0.0, -4.0, 1.0, 0.0, 3.0, -1.0 };
newMatrix(&b, valuesb, 3, 3);
struct Matrix cofa;
matrixCofactor(&cofa, a);
printf("A Cofactor=\n");
matrixToString(cofa);
struct Matrix inva;
matrixInverse(&inva, a);
printf("A Inverse=\n");
matrixToString(inva);
printf("A=\n");
matrixToString(a);
printf("B=\n");
matrixToString(b);
printf("A is %sa square matrix.\n", matrixIsSquare(a) ? "" : "not ");
printf("B is %sa square matrix.\n", matrixIsSquare(b) ? "" : "not ");
printf("The determinant of A is %f.\n", matrixDeterminant(a));
printf("A is %sunique.\n", matrixIsUnique(a) ? "" : "not ");
printf("The determinant of B is %f.\n", matrixDeterminant(b));
printf("B is %sunique.\n", matrixIsUnique(b) ? "" : "not ");
struct Matrix aplusb;
matrixAdd(&aplusb, a, b);
printf("A+B=\n");
matrixToString(aplusb);
struct Matrix asubb;
matrixSubtract(&asubb, a, b);
printf("A-B=\n");
matrixToString(asubb);
struct Matrix ascale;
matrixScale(&ascale, a, 5);
printf("5A=\n");
matrixToString(ascale);
struct Matrix bscale;
matrixScale(&bscale, b, 5);
printf("5B=\n");
matrixToString(bscale);
printf("A and B are %sthe same size.\n", matrixIsSameSize(a, b) ? "" : "not ");
printf("A=\n");
matrixToString(a);
printf("B=\n");
matrixToString(b);
// Test 3D vector math
}
struct _matrix *perceptronLearn(float k, struct _matrix in, struct _matrix t, struct _matrix *w) {
// Declare appropriate matrices
int i, j;
MATRIX *net_j;
MATRIX *delta_w;
delta_w = malloc(sizeof(MATRIX));
memset(delta_w, 0, sizeof(MATRIX));
delta_w->rows = w->rows;
delta_w->cols = w->cols;
delta_w->values = malloc(delta_w->rows * delta_w->cols);
MATRIX *temp;
// Calculate output
net_j = perceptronOperate(in, w);
// Print output and training matrix
printf("\n\nLearn Output:\n");
printMatrix(*net_j);
printf("\n\nTraining Matrix:\n");
printMatrix(t);
// Caclulate error matrix
for (i = 0; i < w->rows; i++) {
for (j = 0; j < w->cols; j++) {
delta_w->values[j + i * w->cols] = k * (t.values[j] - net_j->values[j]) * in.values[i]; // This might go wrong
}
}
// Output error matrix
printf("\n\nDelta_W Matrix:\n");
printMatrix(*delta_w);
// Add to weight matrix, free and return
temp = matrixAdd(*w, *delta_w);
//w falues aren't changed because they are declared and handled in operator.c??
free(w);
return temp;
}
int
McGrip::jointTorqueEquilibrium()
{
Matrix *a, *B;
getRoutingMatrices(&B, &a);
Matrix p(8, 1);
//tendon insertion points
p.elem(0, 0) = 5;
p.elem(1, 0) = 5;
p.elem(2, 0) = 1.65;
//--------------
p.elem(3, 0) = 5;
p.elem(4, 0) = 5;
p.elem(5, 0) = 1.65;
//link length and joint radius
p.elem(6, 0) = getJointRadius();
p.elem(7, 0) = getLinkLength();
//compute joint torques
Matrix tau(6, 1);
matrixMultiply(*B, p, tau);
matrixAdd(tau, *a, tau);
//multiply by tendon force
assert(mTendonVec.size() == 2);
assert(mTendonVec[0]->getName() == "Finger 0");
assert(mTendonVec[1]->getName() == "Finger 1");
double f = mTendonVec[0]->getActiveForce();
for (int j = 0; j < 3; j++) {
tau.elem(j, 0) *= f;
}
f = mTendonVec[1]->getActiveForce();
for (int j = 0; j < 3; j++) {
tau.elem(3 + j, 0) *= f;
}
DBGA("Recovered joint forces:\n" << tau);
//compute joint spring values
assert(numChains == 2);
Matrix k(6, 1);
for (int c = 0; c < 2; c++) {
assert(getChain(c)->getNumJoints() == 3);
for (int j = 0; j < 3; j++) {
k.elem(3 * c + j, 0) = getChain(c)->getJoint(j)->getSpringForce();
}
}
DBGA("Recovered spring forces:\n" << k);
//compute the difference
Matrix delta(6, 1);
k.multiply(-1.0);
matrixAdd(tau, k, delta);
f = delta.fnorm();
int result;
if (f >= 1.0e3) {
DBGA("McGrip joint equilibrium failed; error norm: " << f);
result = 1;
} else {
DBGA("McGrip joint equilibrium success");
result = 0;
}
return result;
}
