Representing a mechanical claw as equations three vectors

Question

I have been trying to find a set of equations for the magnitude and angles that describes 3 connected vectors that lie in the xy plane. These vectors represent a mechanical claw I have that I want to have some very specific properties. I previously came up with what I thought would be representative equations and tried to solve in MATLAB but came up short. (My equations I tried to come up with, which represent the sums of the x and y components and the first and second position)

The initial point of the first vector, in the first position, is located at (0,5.88) and the terminal point of the third vector is located at (0,3.87), the angle of the first vector from this starting point below the horizon in degrees is (44.86-arcsin(0.219/x1)) where x1 is the magnitude of the first vector

And in a second position, where the angles 2 and 3 and the magnitudes of all vectors are the same as in the first problem, where this time the initial point of the first vector is located at (0, 0.374), the terminal point of the third vector is located at (0.91,0.88) and the angle of the first vector below the horizon from the second starting point is (44.86-arsin(2.029/x1)), where x1 is the magnitude of the first vector.

Additionally the problem has constraints that the magnitude of vector 1 must be less than 5 and the magnitude of vector 2 must be less than 3, and the length of the third vector is between 1 and 1.5. and that none of the angles can be over 150 degrees or negative.

I'm having trouble turning this into a set of equations, I think that there are many if not infinite solutions, and its fine if the equations results in some variable being dependent on another, additionally if the equation proves to be unsolvable with the given restrictions, I could do my best to loosen some and would still love to hear a method of solution to this problem.

Answer 1

Rather than use vectors directly we'll use affine matrices which are block matrices of the form $$\begin{bmatrix}A & v\\0 & 1\end{bmatrix}$$ In your specific case $A$ will be $2 \times 2$, $v=[x,y]^T$, $0 = [0,0]$ and $1 = [1]$. This comes with a couple of advantages. Firstly, we can now use matrix multiplication to do vector addition. To see this let $A=I$ the $n \times n$ identity matrix then $$\begin{bmatrix}I & v\\0 & 1\end{bmatrix}\begin{bmatrix}I & u\\0 & 1\end{bmatrix}=\begin{bmatrix}I &v+u\\0 & 1\end{bmatrix}$$ and we can also store local information about transformations we want to impact the vector we've stored in $v$. Say for example we have a $2 \times 2$ rotation matrix $R_\theta$ and consider $$\begin{bmatrix}R_\theta & v\\0 & 1\end{bmatrix}\begin{bmatrix}I & u\\0 & 1\end{bmatrix}=\begin{bmatrix}R_\theta &v+R_\theta u\\0 & 1\end{bmatrix}$$

Firstly, we notice that $R_\theta$ is preserved so the rotation information is encoded. Secondly we can interpret the term $v+R_\theta u$ as first traveling along the arm $v$ then rotating some vector $u$ by $R_\theta$. Perhaps the articulation of that joint may be restricted in some way on $\theta$ in a robotics application.

Now consider a third arm $w$ with rotation $R_\phi$ represented similarly and consider $$\begin{bmatrix}R_\phi & w\\0 &1\end{bmatrix}\begin{bmatrix}R_\theta & v\\0 & 1\end{bmatrix}\begin{bmatrix}I & u\\0 & 1\end{bmatrix}=\begin{bmatrix}R_{\theta + \phi} &w+R_\phi v\\0 & 1\end{bmatrix}\begin{bmatrix}I&u\\0&1\end{bmatrix}=\begin{bmatrix}R_{\theta + \phi}&w + R_\phi v + R_{\theta + \phi}u\\0&1\end{bmatrix}$$

So here we can interpret $w + R_\phi v + R_{\theta + \phi}u$ with $w$ similarly with $w,v,u$ being initial positions then the first arm $w$ has rotated $v$ by $R_\theta$ and $u$ is rotated by $R_\phi$ from $v$ so composing both rotations we have the $R_{\theta + \phi}$ term.

Note that we've put all the trigonometry into the construction of the rotation matrices so we thankfully don't have to look at them once constructed. It just becomes matrix multiplication. Just be mindful of the order, as you'd expect with articulated robot arms or matrix multiplication.

And there is no reason we have to stop with two dimensions either. The OpenGL Transform object is a fully equipped three dimensional version often used in computer graphics. In our applications the $1$ never changes but it's useful when perspective rendering.

Answer 2

Follows a MATHEMATICA a script that describes how to specify the constraints of the problem and also the kinematics needed for the two configurations. The constraints can be manipulated in order to obtain the most convenient results. The objective is to minimize the sum of the lengths of the three sections. The script describes the kinematics and it is self explanatory. I hope. The first setup is described by the symbols $p_1,p_2,p_3,p_4, a_1,a_2,a_3$ and the second by $q_1,q_2,q_3,q_4,b_1,a_2,a_3$.

Follows a plot showing how the angles $a_1,a_2,a_3$ should be considered, clock wise.

Now calling $r(a)=\left( \begin{array}{cc} \cos (a) & -\sin (a) \\ \sin (a) & \cos (a) \\ \end{array} \right)$ a plane rotation of an angle $a$ and considering $\vec l = l(1,0)$ we have

$$ \cases{ p_2 = p_1 + r(-a_1)\vec l_1\\ p_3 = p_2 + r(-a_1-a_2)\vec l_2\\ p_4 = p_3 + r(-a_1-a_2-a_3)\vec l_3 } $$

and then

$$ p_4 = p_1 + r(-a_1)\vec l_1 + r(-a_1-a_2)\vec l_2 + r(-a_1-a_2-a_3)\vec l_3 $$

Now concerning setup $p_1,p_2,p_3,p_4,a_1,a_2,a_3$ we have the restrictions

$$ \cases{ p_1 = (0,5.88)\\ p_4 = (0,3.87)\\ a_1 = 44.86^{\circ}-\arcsin\left(\frac {0.291}{l_1}\right)^{\circ}\\ 1\le l_1\le 5\\ 0.5\le l_2\le 3\\ 1\le l_3\le 1.5\\ } $$

Regarding setup $q_1,q_2,q_3,q_4,b_1,b_2,b_3$ we have the restrictions

$$ \cases{ q_1 = (0,0.374)\\ q_4 = (0.91,0.88)\\ b_1 = 44.86^{\circ}-\arcsin\left(\frac {2.029}{l_1}\right)^{\circ}\\ b_2 = a_2\\ b_3 = a_3\\ } $$

The script performs an optimization procedure to determine the best obedience to the restrictions, trying to minimize the total length of the arms.

Clear[p1, p2, p3, p4, p40, a1, a2, a3, a0, l1, l2, l3, q1, q2, q3, b1]
p1 = {0, 5.88};
p400 = {0, 3.87};
p2 = p1 + l1 {Cos[a1], -Sin[a1]};
p3 = p2 + l2 {Cos[a1 + a2], -Sin[a1 + a2]};
p4 = p3 + l3 {Cos[a1 + a2 + a3], -Sin[a1 + a2 + a3]};
a0 = 44.86/180 Pi;
b0 = a0;
q1 = {0, 0.374};
q400 = {0.91, 0.88};
q2 = q1 + l1 {Cos[b1], -Sin[b1]};
q3 = q2 + l2 {Cos[b1 + a2], -Sin[b1 + a2]};
q4 = q3 + l3 {Cos[b1 + a2 + a3], -Sin[b1 + a2 + a3]};
lengths = {1 <= l1 <= 5, 0.5 <= l2 <= 3, 1 <= l3 <= 1.5, l1 == 0.219/Sin[a0 - a1], l1 == 2.029/Sin[b0 - b1]};
angles = {a1 > 0, b1 > 0, a2 > 0, a3 > 0};
positions = Join[Thread[p4 == p400], Thread[q4 == q400]];
restrs = Join[Join[lengths, angles], positions];
obj = Join[{l1 + l2 + l3}, restrs];
sol = Quiet@NMinimize[obj, {l1, l2, l3, a1, a2, a3, b1}]

p20 = p2 /. sol[[2]];
p30 = p3 /. sol[[2]];
p40 = p4 /. sol[[2]];
q20 = q2 /. sol[[2]];
q30 = q3 /. sol[[2]];
q40 = q4 /. sol[[2]];
gr1 = ListLinePlot[{p1, p20, p30, p40}, PlotStyle -> {Thick, Blue}];
gr2 = ListLinePlot[{q1, q20, q30, q40}, PlotStyle -> {Thick, Blue}];
Show[gr1, gr2, PlotRange -> All, AspectRatio -> 3]

Here

$\text{Angle2}=\left(\pi-a_2\right)\frac{180}{\pi} = 92.917^{\circ}$

$\text{Angle3}=\left(\pi-a_3\right)\frac{180}{\pi} = 103.129^{\circ}$

$\|\text{Vector1}\| = 2.0747$

$\|\text{Vector2}\| = 0.5964$

$\|\text{Vector3}\| = 1.000$