Question: How to solve a heavily nonlinear system of equations?

Hi all,

I'm new to this software, I've started using it to build myself a spacemouse, and figuring out the "inverse kinematics" involved in its working. I've done a little research on how Maple works, I'm fortunate enough to have a student license, but maybe this problem is above my capacity.

Essentially, I want to control a 6-DOF platform by measuring six distances (with magnetic sensors, but that's not relevant, I believe), from three points forming an equiliateral triangle on the top platform, to six points on the bottom platform. So far I've been able to calculate the effect that each of the degrees of freedom has on the platform and on each of this distances that I want to measure.

But that's where the problem lies. Now I have a system of six equations with six variables and a few parameters, a system that's heavily nonlinear, involving a lot of trig functions and multiplications between them. I've tried to "brute force" a symbolic solution by simply typing in: solve(equations, variables) (note that both equations and variables were properly defined) but Maple just ran for a couple of hours without providing an answer. I don't know if this particular application reaches this software's limits, or if it's me who is doing something wrong.
I'll try to provide the maple Worksheet, I'm still not familiar with this site's formatting, sorry in advance:

L := Matrix(3, 3, [[cos(theta)*cos(psi), cos(theta)*sin(psi), -sin(theta)], [sin(phi)*sin(theta)*cos(psi) - cos(phi)*sin(psi), sin(phi)*sin(theta)*sin(psi) + cos(phi)*cos(psi), sin(phi)*cos(theta)], [cos(phi)*sin(theta)*cos(psi) + sin(psi)*sin(phi), cos(phi)*sin(theta)*sin(psi) - sin(phi)*cos(psi), cos(phi)*cos(theta)]]);
T := Matrix(3, 3, [[1, psi, -theta], [phi*theta - psi, phi*psi*theta + 1, phi], [phi*psi + theta, psi*theta - phi, 1]]);
p[ni] := Typesetting[delayDotProduct](R, Vector[column](3, [cos(theta[n]), -sin(theta[n]), 0]), true)

p__n := (T . (p[ni])) + Vector[column](3, [u, v, w])

m[m] := R*Vector[column](3, [cos(phi__m), -sin(phi__m), h/R])

`Δp` := p__n - m[m]


s[n] := sqrt(`Δp` . `Δp`)

theta[1] := -Pi/12;
theta[2] := Pi/12;
theta[3] := (2*Pi)/3 - Pi/12;
theta[4] := Pi/12 + (2*Pi)/3;
theta[5] := (4*Pi)/3 - Pi/12;
theta[6] := (4*Pi)/3 + Pi/12;

phi[1] := 0;
phi[2] := (2*Pi)/3;
phi[3] := (4*Pi)/3;

s[1] := eval(s[n], {phi__m = phi[1], theta[n] = theta[1]})

s[2] := eval(s[n], {phi__m = phi[1], theta[n] = theta[2]})

s[3] := eval(s[n], {phi__m = phi[2], theta[n] = theta[3]})

s[4] := eval(s[n], {phi__m = phi[2], theta[n] = theta[4]})

s[5] := eval(s[n], {phi__m = phi[3], theta[n] = theta[5]})

s[6] := eval(s[n], {phi__m = phi[3], theta[n] = theta[6]})


equations := [-d[1]^2 + s[1]^2 = 0, -d[2]^2 + s[2]^2 = 0, -d[3]^2 + s[3]^2 = 0, -d[4]^2 + s[4]^2 = 0, -d[5]^2 + s[5]^2 = 0, -d[6]^2 + s[6]^2 = 0]

variables := [u, v, w, psi, theta, phi]

solve(equations, variables)


So far this is what I've got, thanks in advance for the help.

EDIT: In the beginning of the worksheet two rotation matrices are declared, L and T. T is a "linearized" version of L, assuming small angles, and I'm using it to simplify the expressions. Still there is no luck.

Please Wait...