1
$\begingroup$

I'm familiar with Chebychev-Kutzbach-Grubler method to determine degree of freedom of a robot arm. But it seems this method fails to calculate the mobility of some parallel robots, as explained here.

However I cannot understand screw theory well and I do not know how to apply it to determine DOF. So I wanna know what is the idea behind screw method to determine DOF? And could anyone explain with a simple example how Screw method works?


EDIT:

Could you please explain how we can determine total DOF of ,for example, SCARA Arm via screw method? enter image description here

$\endgroup$

2 Answers 2

3
$\begingroup$

I will try not to skip too many steps. Assuming a Global coordinate frame at the base and the arm is fully extended along the Y-axis of the base frame. enter image description here

Since SCARA has four joints, we will create four 6D spatial vectors (screws) ${ξ}_{i}$ with respect to a global coordinate frame. Keep in mind that all spatial vectors are described with respect to the global frame. Also ${ξ}_{i}$ will describe the axis around which rotation takes place and along which translation takes place. Basically, we assume a screw motion for every joint along its Z-axis.

The first three elements of vector ${ξ}_{1}$ (for joint 1) will the linear velocity on the screw axis, let's call it ${v}_{i}$. The last three elements are the rotation of the joint with respect to the global coordinate frame, regardless if there are other joints preceding it. Let's call it ${ω}_{i}$ and it will always be a unit vector.$${ξ}_{ι}=\left[{\begin{matrix}{v}_{i}\\{ω}_{i} \end{matrix}}\right]$$

For the first joint the axis of rotation coincides with the global Z-axis. Therefore ${ω}_{1}$ is $${ω}_{1}=\left[{\begin{matrix}0\\0\\1 \end{matrix}}\right]$$

If you are not sure how I came up with it consider the following. if the z axis of the joint was along the y axis of the global frame then the ${ω}_{1}$ would be $${ω}_{y}=\left[{\begin{matrix}0\\1\\0 \end{matrix}}\right]$$. if it was along the x-axis then it would be $${ω}_{x}=\left[{\begin{matrix}1\\0\\0 \end{matrix}}\right]$$. And of course it was along the z axis but pointing downwards then: $${ω}_{-z}=\left[{\begin{matrix}0\\0\\-1 \end{matrix}}\right]$$.

Now let's focus on ${v}_{1}$. We need a point ${q}_{1}$ that will give us the distance of the joint from the origin. The distance is L1 along the z-axis, therefore: $${q}_{1}=\left[{\begin{matrix}0\\0\\L1 \end{matrix}}\right]$$

We rotate ${q}_{1}$ with the unit angular velocity ${ω}_{1}$ (cross product) and we have $${v}_{1}=\left[-{ω}_{1} \times {q}_{1} \right] =- \left[{\begin{matrix}0\\0\\1 \end{matrix}}\right] \times \left[{\begin{matrix}0\\0\\L1 \end{matrix}}\right] = \left[{\begin{matrix}0\\0\\0 \end{matrix}}\right] $$

Therefore $${ξ}_{1}=\left[{\begin{matrix}{v}_{1}\\{ω}_{1} \end{matrix}}\right]=\left[{\begin{matrix}0\\0\\0\\0\\0\\1 \end{matrix}}\right]$$.

Simillarly we have $${ξ}_{2}=\left[{\begin{matrix}{v}_{2}\\{ω}_{2} \end{matrix}}\right]=\left[{\begin{matrix}L2\\0\\0\\0\\0\\1 \end{matrix}}\right]$$

For the third joint the ${ω}_{3}$ is 0 because it is prismatic and ${v}_{3}$ is the unit vector $${v}_{3}=\left[{\begin{matrix}0\\0\\1 \end{matrix}}\right]$$ making ${ξ}_{3}$ $${ξ}_{3}=\left[{\begin{matrix}{v}_{3}\\0 \end{matrix}}\right]=\left[{\begin{matrix}0\\0\\1\\0\\0\\0 \end{matrix}}\right]$$.

For joint 4 $${v}_{4}=-\left[{\begin{matrix} 0\\0\\1\end{matrix}}\right] \times \left[{\begin{matrix}0\\L2+L3\\L1-d1\end{matrix}}\right] = \left[{\begin{matrix}0\\L2+L3\\0\end{matrix}}\right]$$

$${ξ}_{4}=\left[{\begin{matrix}0\\L2+L3\\0\\0\\0\\1\end{matrix}}\right]$$

Now the spatial vectors ${ξ}_{i}$ describe the axes of rotation of all the joints. Keep in mind that some authors have $v$ and $ω$ switch places. For example the paper you are mentioning. This is fine as long as you are consistent throughout.

This example is being worked out in "A mathematical introduction to robotic manipulation" I have linked in my previous answer on page 88.

These are the basics of the screw theory description. Similarly we can create a wrench 6D vector φ with the linear and the angular (torques) forces. As you can tell by now, screw theory is a method to describe the manipulator (like the DH parameters) and not a mobility formula like the Chebychev-Kutzbach-Grubler method. The paper you are referencing stacks the spatial vectors (it calls them twists) ${ξ}_{i}$ and the wrenches ${φ}{i}$ to create a twist matrix and then analyze their constraint using the union and the reciprocal of the twists. Those are vector functions that the screw theory can take advantage of.

I am not sure if my answer helps you but the paper you are mentioning requires a relatively strong understanding of screw theory and vector algebra to make sense.

Please let me know if you need more clarifications.

Cheers.

$\endgroup$
1
  • $\begingroup$ Thank you so much for clarifying things. I also find another good reference for DOF methods.link $\endgroup$
    – sci9
    Commented May 12, 2017 at 1:14
0
$\begingroup$

The most popular introductory text for screw theory on robotics is "A mathematical introduction to robotic manipulation" which can be found here for free. A more mathematically advanced text (perhaps too advanced) is "Geometric Fundamentals of robotics" but it is not free. Another text discussing screw theory is "Rigid Body Dynamics algorithms" by Roy Featherstone. You might want to check his website, he has some introductory material before he goes to the full-blown dynamics.

I am probably oversimplifying things, but a screw is a 6D vector that has the linear and the rotational velocity of movement and is accompanied by a wrench which is a 6D vector with the linear forces and the torques. They might look like they are two 3D vectors stacked together, and is some cases behave like that, but they have their own vector properties.

The paper you are referencing should make more sense once you read Chapter 2 and 3 of the book I am linking.

Hope this will give you a good starting point.

$\endgroup$
1
  • $\begingroup$ Thank you for answer, but could you give an example how this theory is used to determine DOF? Please see the edited post. $\endgroup$
    – sci9
    Commented May 11, 2017 at 3:18

Your Answer

By clicking “Post Your Answer”, you agree to our terms of service and acknowledge you have read our privacy policy.

Not the answer you're looking for? Browse other questions tagged or ask your own question.