Heres the Matlab code, I start with x and y and when I pass it through IK and Fk I get back the correct x and y thus I am confident that IK and Fk are correct.

x = 10; y = 10; z = 0;
a2 = 7.5; a3 = 9;
r = sqrt(z^2 + y^2);

th1 = atan2(y,x);
th3 = acos((r^2 + x^2 - (a2^2 + a3^2))/(2*a2*a3));
th2 = atan2(x,z) - atan2(a3*sin(th3), (a2 + a3*cos(th3)));
T0 = compute_dh_matrix(0,0,0,0);
T01 = T0*compute_dh_matrix(0, -pi/2, 0, th1);
T02 = T01 *compute_dh_matrix(7.5, 0, 0, th2-pi/2);
T03 = T02 *compute_dh_matrix(9, 0,0 , th3);

%ans =
%    10.0
%    10.0
%       0

I have tower pro micro servos SG90 for base, shoulder and elbow joints.I am giving power using a mobile charger which outputs 5V and 2A.Now I want to try drawing some simple shapes with this robot I tried giving it the sequence of x and y locations with z=0, but the results seem not good.Do I need another DOF on the end effector?Is there any way to map x-y to the theat_start - theta_end for every angle?This is the diagram of the robot, I have a small change as my zero position is upright not with a bend like below but the rotation and angles are the same.enter image description here.

Now I am trying to draw in X, Y plane as Z is zero all the time.a2 and a3 are the link lengths.I don't know how to control the speed of the servos I just hacked the sweep code from Arduino.I just want to draw some basic shapes or a line, I tried giving (9,9), (10,10) and (11,11) thinking it will draw a line.

enter image description here

These images show my setup in detail.I haven't taken all the offsets yet.I was getting close but not exact x and y that I entered into the IK->FK functions so I thought I could just work with less accuracy to get results quick.If you can help with IKfor offsets it would be great too.

I really hope this is a problem that you guys consider as per the rules and as I have done work on FK and IK I hope this question makes sense.Thank you.

  • 1
    $\begingroup$ What do you mean when you say the results not looking good? $\endgroup$ Aug 24, 2017 at 14:41
  • $\begingroup$ In what plane is the drawing surface? How far apart are the $(x,y)$ locations of the points to draw? What are the rough dimensions of the arm, (i.e. link length). How fast are you moving the servos? $\endgroup$
    – Ben
    Aug 25, 2017 at 13:20
  • $\begingroup$ I am sorry I meant to draw X,Y plane as Z is zero all the time.a2 and a3 are the link lengths.I don't know how to control the speed of the servos I just hacked the sweep code from Arduino.I just want to draw some basic shapes or a line, I tried giving (9,9), (10,10) and (11,11) thinking it will draw a line.Thank you. $\endgroup$ Aug 28, 2017 at 13:13
  • $\begingroup$ I did a similar simulation using SolidWorks: youtube.com/watch?v=crJXUlzJ918 $\endgroup$
    – LCarvalho
    Feb 19, 2018 at 20:02

2 Answers 2


To explain the reason why you are getting a bad output you need to understand trajectory planning.

  1. So you have now done the FK and IK for the robot, however now to be able to do the tasks that you have mentioned, the biggest problem is that your end effector needs to traverse a smooth continuous path.

So feeding the robot with the end points it not enough, you need to be able to generate a trajectory using these end points. The equation for a cubic polynomial is as follows.

enter image description here

You can fit the three points on this curve by analytically finding the coefficients by feeding boundary conditions for velocity, acceleration and position. This can be done using MATLAB.

Now the program will feed your inverse kinematics calculating program with the points on the trajectory. The gap between two points will depend upon the step that you have to determine.

Now take the case of the straight line motions that you want. Feed the robot with intermediate points in steady increments, that should do the trick. If it does not work, then you may consider all my other suggestions in this answer.

  1. Also your joints need to complete the task at a speed such that the motion of all the joints start and end at the same time, this enables smoother movement of your joints, again it involves trajectory planning concepts and a whole lot of theory. But you can consider this as low priority since you have individual actuators for every joint and no feedback loops.

  2. Btw make sure the points you are feeding are not on the edge of your work space, in that case you will have a singularity and the robot would not achieve the position correctly.

  3. For best results replace those cardboard stuff with something more solid like some plastics using 3D printing. And then calculate mass and moments of inertia of the links and everything to calculate how much torque is required to be able to push the pen down on to the paper hard enough for the pen to create a mark. For this you need to learn robot dynamics which involves use of manipulator jacobian and so on..

Hope this helps.


If you consider your robot's pose in the plane of the upper arm and forearm, you'll see that you have a 2dof planar mechanism which gets rotated by the waist joint. With only 2 dof in that plane, you can specify the x and y coordinates where you want to draw, but you cannot independently change the angle of the tool tip - it will be whatever it needs to be to reach those coordinates. Your only options are to draw with a pen or brush that has a very large drawing tip (and hopefully circular or spherical), or to add a single wrist motor with which you can change the angle of the pen. You'll have to solve for 4 dof inverse kinematics in this case.

  • $\begingroup$ Thank you.But I don't know much about the servo velocities I basically hacked the sweep code in Arduino.Do I consider converting to a configuration space and then executing the the path in cspace. $\endgroup$ Aug 28, 2017 at 13:10
  • $\begingroup$ That is exactly what the inverse kinematics does for you. You can define your path in task space, and the inverse kinematics translates those motions into configuration space motions. $\endgroup$
    – SteveO
    Aug 28, 2017 at 13:33
  • $\begingroup$ I thought it will draw a line when I give angles corresponding to x,y points (9,9), (10,10) and (11,11) , but it executed some weird motions it was not moving from one point to another.I am just giving the angle as in Arduino sweep program.Thank you for staying with me, I have taken MOOCs and online materials on Robotics but not so good on the practical side because I never had the luxury of buying all the stuff, communities like this help me a lot thank you very much. $\endgroup$ Aug 30, 2017 at 5:02
  • $\begingroup$ Can you add a description of what you are obseving to your question? Or, better yet, add a video of what the robot does? Also, try setting the points closer (perhaps go in 0.1" increments) and see what the robot does. $\endgroup$
    – SteveO
    Aug 30, 2017 at 12:13
  • $\begingroup$ Yesterday I noticed it drew small arcs when moving from one point to another, I have added more images to get a clear picture.Thank you. $\endgroup$ Aug 31, 2017 at 9:24

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.