4 DoF Robot Arm Pick & Place Color Sorter With Inverse Kinematic

In this project i build a 4 DoF robotic arm to do object sorting based on color. To build robotic system (at least like this project) it is not only programming and electrical field, but also required mechanical understating. Kinematic is a subfield in classical mechanics that describes the motion of points, bodies (objects), and systems of bodies (groups of objects) without considering the forces that cause them to move. Then what the correlation of this mechanics subfield study to programming robot? programming a robot which mean we give a 'set of instruction' where it moves on the environment, with any kind of tools and variety of job, but still, the principle is basically constructing the movement trajectory of a robot, then later we can come up for the programming.

In robotics nomenclature "end effector" is the device at the end of a robotic arm. The end effector means the last link (or end) of the robot. At this endpoint, the tools are attached. In a wider sense, an end effector can be seen as the part of a robot that interacts with the work environment.

By understanding definition of End Effector from previous paragraph now lets model the robot arm end effector position as cartesian coordinate system, because robot work in 3 dimensional space then its cartesian coordinate axis consist of x axis, y axis, and z axis. In this case the robot arm is 4 DOF and the tip (end effector) is gripper module, so by modeling the position of this gripper on environment now we can have a method to determined where it moves in order to do a job we want. Let say we want this robot arm to pick up object at certain position, that position is then defined by value of each x, y, z axis (the unit of axis value are same as distance unit, e.g. mm) then how much the angle of each joint should rotate in order the end effector of the robot reach that point are solved by implementing some mathematical equation (geometric formula), this is what Inverse Kinematic method are, a method that converting cartesian coordinate of robot end effector position to polar coordinate (angle) of each joint.

Because each joint of this robot arm driven by electric motor (servo motor) then the value of angle each joint calculated from geometric equation is the value of how much each these servo motor on the system should rotate in order to make the gripper reach the position or move according to trajectory movement of the end effector (gripper) as we want.

Note: the link to these parts i bought is from local online shopping website in Indonesia (Tokopedia) and some bought from local retail stores. if you want to find the parts at region you're living my tips is search with the keyword of the parts i list then match the images of parts, hope exact same part i used available at your region local stores (online or retail), if not you need improvised a bit.

1. Wood board 50x50cm.
   This is optional parts, if you wish to place your robot and component on your desktop is also fine.
2. Arduino UNO Board.
   This part should be available everywhere you live.
3. Gripper Module.
4. Robot chassis.
   Consist of: L-Bracket, U-Bracket, Universal Servo Mount.
5. 3x TowerPro MG996R Servo Motor (clone version).
   Make sure buy 180 degree servo, not continuous (360 degree) version.
6. 2x JX PDI-6221MG-180 Servo Motor.
   Make sure buy 180 degree servo, not continuous (360 degree) version.
7. Optional metal servo disk.
   This is accessories component for servo shaft for joint or mounting to other component. when you buy servo motor usually you get included component but made of plastic, so maybe to save budget just use the included mount, the metal version are aftermarket accessories.
8. 12V 10A AC-DC Power Supply.
   Don't forget 3 wire cable and male plug socket.
9. HC-SR04 Ultrasonic Sensor.
10. TCS230 Color Sensor.
11. PR15 DC to DC Step Down Buck Converter 12A 100W.
    This part is to step down voltage of power supply to 6.5V to supply power for servos and Arduino board.

12. XL4015 DC to DC Step Down Buck Converter 5A.
    This part is to step down voltage of power supply to 5V to supply power for sensors.

13. Block of wood, or other object for demonstration (at least 3 piece, Blue, Green, and Red) as shown on the YT video.

14. Bolt, nut, and washer.
    I even don't know how much bolt, nut, and washer used for this project, so its self adapted

15. Jumper cables.
    To connect all the components

The tools needed for this project:

• Wood saw (to make the wooden board)
• Power drill
• Screw driver
• Solder and its lead
• Multimeter
• Laptops/computers (any OS)
• Arduino IDE Software.
  If you Windows user you can download on microsoft store apps. Or https://www.arduino.cc/en/software

Most of the tools used for making the wooden board anyway, like drilling to make hole for bolting the chassis to the wooden board. Other than that is for electrical component connection to each other by soldering some jumper cables

Before assembling the parts or component it is better to understand what exactly this robot arm are supposed to do. In order to make this robot arm as pick and place object sorter based on color then first locate or determined position of where the object is coming and need to pick up, then where it should be moved according on its color. The position where the robot grip and pick up the object is where the sensors are placed, HC-SR04 ultrasonic sensor detect the presence of object and TCS230 identified what color it is, if it was one of the color it programmed to respond (red, green, or blue) then the robot arm moves that object to designated location for each color.

The vertical direction are y axis, x and z axis are the surface, the z are 'forward' direction, x are 'side' direction. Zero point for y axis is the surface, x and z zero point is where center of first joint. First joint is rotational joint which perpendicular to its surface, or in other word first joint is rotational at y axis.

Based on my wood board platform position to pick up object at {x = 180, y = 0, z =10}, blue object destination {x = -50, y = 0, z = 250}, green object destination {x = -150, y = 0, z = 250}, and red object destination {x = -150, y = 0, z = 150}. For example visualizing end effector movement trajectory when blue object detected shown as the 2nd picture above.

By modeling the movement of robot end effector with cartesian coordinate system general overview writing program for operating the robot is you just need to change the value of x, y, and z position of the end effector. sounds easy, right? But, programming servo motor with Arduinos Servo.write() you need to input degree value how much the servo you want to rotate. So that's why we need implement some geometric formulas that could convert position (given value of x, y, and z) of end effector then calculate to give output value of angle each joint should rotate. Then we should go to next step to understanding some Kinematics formulas.

Given angle value each joint of group of bodies or mechanical system then calculated to give output of position coordinates where the tip of the system are expected is called 'Forward Kinematics'. Forward kinematics of course sounds not suitable to operate a robotic system, we want the opposites, determined the path, or trajectory of robot end effector where to move, then calculate how much angle each joint should rotate. because when programming a robot we need to send 'signal' from controller board to the actuator, the signal contain value of information how much angle servo motor rotate that drive the robot joints, hence why we need Inverse Kinematic.

In this project, the inverse kinematic equation will use a geometric method to determine the joint angles of the robot arm to reach desired end effector position. For reducing the complexity, we reduce the problem from 3D to 2D point of view. Now, lets assume we look at the robot arm from above, which represent plane (x, z). To make the gripper (end effector) reach desired coordinate position the first joint should rotate at variable declared as "Theta 1", beside the first joint rotation, the arm need to span to make end effector reach desired coordinate position. Let's symbolize variable how far the arm need to span as "D". Calculation for Theta 1 and D is shown on picture above.

In the previous section we have implemented formula from geometric principle which can be used to calculate the rotation angle of joint 1 (Theta 1), now we are analyzing another 2D viewpoint to determine the equation to find the other three joint angles (Theta 2, Theta 3, and Theta 4). In short, how much angle joint 2, 3, and 4 needed to rotate to make the arm form span from length that calculated before (D).

Calculation to find Theta 2 (joint 2 rotation angle) and Theta 4 (joint 4 rotation angle) when position of gripper (end effector) lower than joint 2 position in y axis direction a bit require different formula than if end effector position higher than joint 2 position.

This inverse kinematic formula only for to make link 4 (L4) parallel to the ground/surface, but since this robot is 4 Degree of Freedom (4DoF) so there is flexibility to not only determined position where its end effector should move/reach but also the orientation of the end effector, but it require different formula.

Anyway there is 6 Degree of Freedom 'law' of rigid body movement in three-dimensional space, there are; translational move along axis x, y, z (3 DoF), and rotational move along axis x, y, z (3 DoF), so total is six. This robot end effector capable all three axis direction translational move freedom, and one rotational freedom, so hence why it is 4 DoF robot.

As of assembling component i think it is straight forward process. Basically you just bolt the bracket parts each other and the servos. If you need guide for layout of the chassis bracket component you can see 3D view of the robot design here.

An 180 degree servo motor mean it can only rotate from 0 degree position to 180 degree position, so when you install the shaft mount disk thingy (silver or black round parts on the picture) make sure it is at calibrated position. Let say when you at process to install joint 2 servo mount on the joint 1 servo disk that you already know at what degree joint 1 servo at.

Rotation direction of servo motor shaft usually counter clockwise from 0 to 180 degree, that if from perspective when you look at the shaft, but when you look from the opposite sides from 0 to 180 is clockwise, so pay attention of shaft rotation direction relative to where the servo installed.

The wood board is just as platform of robot demonstration where the arm itself bolted to the board (servo mount of joint 1) and other electrical component. in this process you mostly drill some 3mm hole for bolt those component.

Easiest way to explain assembling electrical component is visualized by wiring diagram from picture above.

• AC-DC 12V Power Supply.
  If you see a terminal (Philips head style bolt) that parallel to "L N" label that the input electric from your building wall socket, so you need 3 wires, 1 to L, 1 to N, and 1 other for ground, the ground terminal is beside the L and N terminal. Next to the right terminal of L N and Ground, is the output terminal. Electricity from this terminal is in form of DC current. "COM" label terminal is the negative and "V+" terminal is the positive.
• PR15 Buck Converter.
  Use some cable from DC output (V+ and COM) terminal of the power supply to "IN" labeled terminal on the PR15. Pay attention to the positive and negative. This converter also have output terminal that labeled as "OUT"
• XL4015 Buck Converter.
  Same like PR15, output terminal of 12V power supply connect to "IN" labeled XL4015 module, also don't forget for the positive and negative. This converter also have output terminal that labeled as "OUT".
• HC-SR04 ultrasonic sensor.
  There are 4 pin in this module, GND, Echo, Trig, and Vcc. Output terminal of XL4015 converter connect to VCC and GND, VCC is positive GND is negative. Echo pin connect to pin 7 of Arduino UNO board, Trig pin connect to pin 8 of Arduino UNO board.
• TCS230 color sensor.
  There are 8 pin in this module. Positive and negative output of XL4015 connect to VCC and GND on this component. VCC is positive, GND is negative. Then, pin S0 connect to pin A1 on arduino, pin S1 connect to pin A2 on arduino, pin S2 connect to pin A3 on arduino, pin S3 connect to pin A4 on arduino, and pin OUT connect to pin A5 on arduino.
• Servo motors.
  Servo motor have 3 wires, black (or brown), red, and yellow. Red and black are where power comes in, which mean positive output terminal of PR15 converter connect to red wire, negative output terminal of PR15 connect to black or brown wire. Yellow wire are 'signal' wire from controller (arduino board). yellow wire for joint 1 connect to pin 9, for joint 2 connect to pin 5, for joint 3 connect to pin 10, for joint 4 connect to pin 6, and servo to drive grippe connect to pin 3.

Further detail on TCS230 color sensor and HC-SR04 ultrasonic sensor will updated to separate instructable post.

From step 3 : How it works, we can summarized what operation this robot arm do in form of flow chart above. The flow chart is like general overview of the robot programs, then the flow chart could help and guide to write the program of this color sorter robot operation and make it working.

And then here the .ino Arduino C++ based code file which is the program to run this robot arm system.

Thanks for visiting this instructables tutorial, feel free to give correction and feedback if anything miss or wrong. and if you hope i could build other cool robotic project you can support me by giving donation to paypal.me/ilhamdefra