0
$\begingroup$

The triangles used for angle calculations

I am trying to control a Dobot arm. The arm moves with angles whereas I need to work with cartesian coordinates. From inverse kinematics equations and polar coordinates I have implemented x,y and z coordinates working very well on their own. But I can not combine the coordinates in order to work all together. When I add them up it is not going to the desired place. How can I combine these coordinates? I got some help from (https://github.com/maxosprojects/open-dobot) but could not manage to successfully move dobot.

Edit: I've written the codes in Qt and also I've added the triangles used for angle calculations.

//foreArmLength=160mm rearArmLEngth=135mm

float DobotInverseKinematics::gotoX(float x) //func for x-axis
float h=qSqrt(qPow(lengthRearArm,2)-qPow(x,2)); //height from ground
QList<float> zEffect=gotoZ(h); //trying to find the effect of x movement on z-axis
float cosQ=h/lengthRearArm; //desired joint angle
float joint2=qRadiansToDegrees(qAcos(cosQ));    
//move in range control
if(joint2 != joint2)
{joint2=0;
    qDebug()<< "joint2NAN";}
return joint2;

QList<float> DobotInverseKinematics::gotoY(float y) //func for y-axis
QList<float> result ;
float actualDist=lengthForeArm+distToTool; //distance to the end effector
float x=(qSqrt(qPow(actualDist,2)+qPow(y,2)))-actualDist; //calculating x movement caused by y movement
float joint1=qRadiansToDegrees(qAcos(actualDist/(actualDist+x))); //desired joint angle
float joint2=gotoX(x);  //the angle calculation of y movement on x axis
if(joint1 != joint1)
{joint1=0;
    qDebug()<< "joint1NAN";}
result.append(joint1);
result.append(joint2);
return result;

QList<float> DobotInverseKinematics::gotoZ(float z) //func. for z-axis
QList<float> result ;
float joint3=qSqrt(qPow(160,2.0)-qPow(z,2.0))/ 160; //desired joint angle
float temp=160-qSqrt(qPow(160,2.0)-qPow(z,2.0));
float joint2=qSqrt(qPow(lengthRearArm,2)-qPow(temp,2.0))/lengthRearArm; //desired joint angle
if(joint3 != joint3)
{joint3=0;
    qDebug()<< "joint3NAN";}
joint2=qAcos(joint2)*(180/M_PI);
joint3=qAcos(joint3)*(180/M_PI);
result.append(joint2);
result.append(joint3);
return result;
$\endgroup$
  • $\begingroup$ Could you please edit your question to include the work that you've done so far? I took a look at the Dobot documentation and didn't see anything specifically about kinematic lengths or reference datum for individual joints. $\endgroup$ – Chuck Aug 17 '16 at 13:48
  • $\begingroup$ @Chuck I've edited the question sorry for the previous format I was in a hurry I've forgotten to add the codes $\endgroup$ – hatoz Aug 18 '16 at 4:46
  • $\begingroup$ For others trying to help, it seems that the Dobot people are trying to do a simplified analytic inverse kinematics by 'converting x and y to polar coordinates to get angle and radius'. @hatoz, There are two steps to the solution: f(x,y)->joint1,radius f(radius,z)->joint2,joint3. $\endgroup$ – hauptmech Aug 18 '16 at 10:48
  • $\begingroup$ @hauptmech thx for the reply but I am trying to calculate joint1 from x and y by getting simply atan(y/x) but the result does not seem to be correct $\endgroup$ – hatoz Aug 18 '16 at 12:16
  • $\begingroup$ atan2(y,x) always. You can Google why. $\endgroup$ – hauptmech Aug 18 '16 at 12:55
1
$\begingroup$

$a_1, a_2$ : Link lengths

$p_x,p_y$ : Target $x$ and $y$ coorditate

$\theta_1=atan2(p_y,p_x)$

$p_r=\sqrt{p_x^2+p_y^2}$ : Target radial distance

$p_z$ : Target $z$ coorditate

$\kappa=\frac{p_r^2+p_z^2-a_1^2-a_2^2}{2a_1a_2}$

If $\kappa>1$ the position cannot be reached.

$\theta_3=cos^{-1}(\kappa)$

There are 2 solutions, elbow up or elbow down. Below use $\theta_3^*=\theta_3$ or $\theta_3^*=-\theta_3$

$\Delta=a_1^2+a_2^2+2a_1a_2cos(\theta_3^*)$

$c\theta_2=\frac{p_r(a_1+a_2cos(\theta_3^*))+p_za_2sin(\theta_3)}{\Delta}$

$s\theta_2=\frac{p_z(a_1+a_2cos(\theta_3^*))-p_ra_2sin(\theta_3)}{\Delta}$

$\theta_2=atan2(s\theta_2,c\theta_2)$

These are joint angles. To get motor angles, you will have to adjust the angles based on how the motors are connected to the robot. $\theta$ is 0 when horizontal, $\theta_3$ is 0 when straight (in-line) with Link 2.

A detailed explanation can be found in Robot Analysis by Tsai. ISBN 9780471325932 pp 71

$\endgroup$
  • $\begingroup$ thank you for your answer, I've implemented it but I can't get a result since the cos value is calculated as something bigger than 1, also I've assumed these a2 and a1 lengths are the arm lengths am I right? $\endgroup$ – hatoz Aug 19 '16 at 7:30
  • $\begingroup$ Sorry, I was missing a parenthesis. I added a note on how to detect when there is no solution and how to get both solutions. Sorry I don't have time to do a diagram. $\endgroup$ – hauptmech Aug 19 '16 at 11:32
  • $\begingroup$ I've implemented your solution this time run without errors, but the angles are really bigger than the desired values I could not find where the mistake is, there is nearly a 2 cm slip. Do you have any ideas why something like that may happen? Thank you for your effort. @hauptmech $\endgroup$ – hatoz Aug 22 '16 at 5:54

Your Answer

By clicking “Post Your Answer”, you agree to our terms of service, privacy policy and cookie policy

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