Development of a 4-DOF Serial Robotic Manipulator for

Pick-and-Place Applications
Kai Nakamura, Owen Sullivan, and Evan Carmody

Abstract—This paper presents our development of a 4-DOF

serial robotic manipulator designed for pick-and-place tasks. We
discuss robot kinematics, camera calibration, transforming 2D
pixel coordinates into 3D world coordinates, object detection
and classification, and object localization challenges. We conclude
with insights about our final system and discuss is applicability
in the field of robotics.

I. I NTRODUCTION
Our primary goal was to create a ball-sorting robot using
a robotic arm and a camera. A variety of colored, 3D-printed
balls are arbitrarily placed on the workspace, and the robot is
tasked with locating the balls and determining the trajectory
to grab and sort the different colors. This project required
us to explore and implement forward kinematics, inverse
kinematics, trajectory generation, and a robust computer vision Fig. 1: Denavit-Hartenberg (DH) Reference Frames of the
system. OpenMANIPULATOR-X
The robotic arm we used was the OpenMANIPULATOR-
X, an open-source and low-cost serial manipulator [1]. The
OpenMANIPULATOR-X has four revolute joints and four Multiplying all of the transformation matrices together will
degrees of freedom. The camera we used was a standard USB produces a single transformation matrix between the base
webcam [2]. The camera we picked used a wide-angle fisheye frame of the robot to the end-effector (1). This final transfor-
lens which we had to account for in Section II-C. mation matrix is the forward kinematics solution for our robot
since it contains the end-effector’s position and orientation
II. M ETHODOLOGY relative to the base frame of the robot. Equation 2 shows
A. Forward Kinematics the forward kinematics solution for the position of the end-
effector. Note that s and c are short for sin and cos, and that
In order to control our robotic arm and move it around the
θ23 and θ234 are short for θ2 + θ3 and θ2 + θ3 + θ4 .
workspace, we needed to solve the forward kinematics of our
robot so we could determine the end-effector position given
T05 = T01 · T12 · T23 · T34 · T45 (1)
the current joint angles. We used the Denavit-Hartenberg (DH)
convention to model the forward kinematics of our arm [3]. We
started by assigning frames of reference to each of the joints  
c(θ1 )[133c(θ234 ) + 124c(θ23 ) + 130s(θ2 )]
(F1 , F2 , F3 , and F4 ) as well as the base of the robot (F0 ) and
p⃗ee =  s(θ1 )[133c(θ234 ) + 124c(θ23 ) + 130s(θ2 )]  (2)
the end-effector (F5 ) as shown in Fig. 1. Note that the frames
96.3 + 133s(θ234 ) + 124s(θ23 ) + 130s(θ2 )
F0 and F1 coincide with one another. From these frames, we
found the DH parameters of the OpenMANIPULATOR-X and B. Inverse Kinematics
used them to calculate the transformation matrices between Forward kinematics is great for figuring out where the end-
each of the joints (Table I). effector is from the given joint angles. But in order to pick
TABLE I: Denavit-Hartenberg (DH) Parameters of the up balls at a given position, we need to be able to work
OpenMANIPULATOR-X backwards to find the desired joint angles. This is where
inverse kinematics comes into play.
Unfortunately, calculating the inverse kinematics is a lot
Tii+1 θ [rad] d [mm] a [mm] α [rad]
π
more complicated because there might be multiple or even
T12 θ1∗ 96.326 0 2 infinite solutions for a single position. There might even be
T23 θ2∗ + tan−1 ( 128
24
) 0 130.23 0 no solutions if the position isn’t reachable. But, the general
128
T34 θ3∗ − −1
tan ( 24 ) 0 124 0 idea is to use trigonometry to calculate possible joint angles,
T45 θ4∗ 0 133.4 0 and then plug those values back into the forward kinematics
to see which configurations actually work.
 Since the given position has three coordinates (x, y, and z)
and there are four unknown joint angles, the angle α is also x24
Dϕ = cos(ϕ) = p 2
provided to make the problem solvable where α is the angle 2
x24 + y24
between the end-effector and the xy-plane. q (6)
From Fig. 2: ϕ = atan2(± 1 − Dϕ2 , Dϕ )

p θ2 = ϕ − β (7)
r= x2c + yc2
r cos(θ1 ) = xc
xc θ4 = α − θ2 − θ3 (8)
D1 = cos(θ1 ) = (3)
r q
θ1 = atan2(± 1 − D12 , D1 )

Fig. 3: Diagram of Joints 2, 3, and 4 from the XR-Plane

Equations for θ1 , θ2 , θ3 , and θ4 all use atan2 to solve for

all possible solutions of inverse kinematics. Not all candidates
are valid solutions, in fact most are not. To determine which
combinations of joint angles are actually valid, we plug the
candiates back into the forward kinematics to see if the joint
Fig. 2: Diagram of the Robot at an Arbitrary Pose angles bring the end-effector to the desired position.
From Fig. 3: C. Camera Calibration
Proper camera calibration is necessary to ensure accurate
x24 = r − L4 cos(α) and reliable visual perception of the workspace. The first step
of the camera calibration process is to remove any distortion
z24 = zc − L1 − L4 sin(α)
that the camera lens applies to the image. The camera we
Using the Law of Cosines: used has a wide-angle fisheye lens that produced significant
distortion near the edges of the image. This distortion can
L22 + L23 − (x224 = Z24
2
) make objects appear stretched or warped which causes inac-
cos(π − θ3 ) = curacies in object detection and localization. Notice how the
2L2 L3
straight red lines in Fig. 4 don’t line up with the edges of the
L2 + L23 − (x224 = Z242
) (4)
D3 = cos(θ3 ) = − 2 checkerboard.
2L L
q 2 3 To correct for this distortion, we used MATLAB’s Camera
θ3 = atan2(± 1 − D32 , D3 ) Calibrator app to calculate our camera’s intrinsic parameters
and remove the effects of lens distortion. The Camera Calibra-
Using the Law of Cosines: tor uses several calibration images each taken from a different
perspective to estimate various parameters such as the focal
L22 + (x224 = Z24
2
) − L23 length and lens distortion coefficients.
Dβ = cos(β) = p The Camera Calibrator detects the intersection points of the
2L2 x224 = Z24 2
(5)
q checkerboard pattern and uses these to establish a correlation
β = atan2(± 1 − Dβ2 , Dβ ) between 2D pixel coordinates and 3D world coordinates. The
 Fig. 4: Distorted image taken by a fisheye lens camera

app estimates the camera parameters by minimizing reprojec-

tion error which is the difference between the observed image
points and their projected world coordinates. For our set of
calibration images, the Camera Calibrator app was able to
estimate camera parameters that reduced the mean reprojection
error per image down to 0.51 pixels (Fig. 5).

Fig. 6: Estimated locations of the camera from the Camera

Calibrator app

Fig. 5: Mean reprojection error of calibration images

Fig. 7: Undistorted image of the checkerboard

The Camera Calibrator’s estimated locations of the camera
from the calibration images can be seen in Fig. 6. These
estimations closely mirror the real world positions that our D. Camera-Robot Transformation
calibration images were taken from as we moved our camera
In order for the robot to pick up an object, it has to know
around the checkerboard.
where the object is in respect to its origin. However, world
Once we had calculated the intrinsic parameters of the cam- points obtained from the calibrated camera lie in the reference
era, we could use them to remove the fisheye lens distortion. frame of the checkerboard, Fchecker , and not in the reference
Notice how the straight red lines in Fig. 7 now line up with frame of the robot, F0 (Fig. 8). Points with respect to the
the edges of the checkerboard, unlike how they did previously checkerboard can be transformed into points with respect to
in Fig. 4. the robot by finding the transformation matrix that maps frame
Using the parameters of our calibrated camera, we could 0 of the robot to the checkerboard frame, T0 checker .
then create a transformation matrix that converts 2D pixel The transformation matrix between the robot and checker-
coordinates from an image and translates them into 3D world board can be calculated by finding the rotation and translation
coordinates that lie on the xy-plane of the checkerboard. This between the robot and checkerboard. Equation 9 shows the
checker
transformation matrix is called, Timage , since it maps pixel rotation matrix, R0checker , obtained by visual inspection of the
coordinates of the image to world points on the checkerboard. robot frame and checkerboard frame (Fig. 8). Equation 10
shows the translation matrix, p⃗0checker , obtained by measuring each ball and classify its color. The gray colored balls proved
the distance between the origin of the robot and the origin challenging to detect since they blend in with the white and
of the checkerboard. Equation 11 shows the final transforma- black checkerboard, unlike the other brightly colored balls.
tion matrix, T0checker , obtained by combining R0checker and This required us to develop a robust image pipeline capable
p⃗0checker . of identifying all five colors.
 
0 1 0
R0checker = 1 0 0  (9)
0 0 −1
 
113
p⃗0checker = −95 (10)
0
 
0 1 0 113
1 0 0 −95
T0checker =  0 0 −1
 (11)
0 
0 0 0 1

Fig. 9: Red, orange, yellow, green, and gray colored balls

However, there are some objects that even the most robust
image pipelines can’t detect. We were able to identify the gray
balls because they can still be differentiated against the white
and black checkerboard. It would be near impossible to detect
a white ball or black ball since it would completely blend in
with the checkerboard. Different colors that are very similar
can also cause difficulties if they are not easily discernible
Fig. 8: Reference frames of the robot (F0 ) and the checker- from one another.
board (Fchecker ) 1) Brightness Equalization: The first step of our pipeline is
to equalize the brightness of the raw image. Later stages in the
With T0checker from Section II-D and Timage
checker
from Section pipeline rely heavily on predefined color thresholds which can
II-C, 2D pixel coordinates from the image can now be fully be thrown off if the brightness of the image changes due to
converted into 3D world coordinates in the reference frame of the ambient light in the room. To account for this, we apply
the robot (12). a histogram equalization to evenly distribute the brightness
of the image (Fig. 10). Brightness equalization will make an
image
p⃗0 = T0checker · Tchecker · p⃗image underexposed image brighter and an overexposed image darker
so that the vision pipeline stays consistent no matter what the
checker 0
lighting conditions are (Fig. 11).
p⃗image = Timage · Tchecker · p⃗0 2) Undistortion: We then undistort the fisheye lens using
the calibrated camera parameters from Section II-C. It is
0
p⃗image = Timage · p⃗0 (12) important to perform this step after the brightness equalization
so that the black edges introduced from undistortion don’t
E. Object Detection and Classification influence the brightness histogram.
Detecting objects from an image and classifying them re- 3) Image Masking: Next, the image is color masked to
quires several stages of image processing to extract meaningful separate out the balls from the background. This operation
information that can be used for decision-making. Each step of is done in the HSV color space which we found was the most
an image processing pipeline performs a distinct operation that intuitive to work with. There are two filters applied to the
enhances or extracts desired information or reduces irrelevant image, one for the colored balls and one for the gray ball.
details. Both filters are then combined with one another to get a mask
Our robot was designed to pick up small colored balls that contains all five colored balls.
(red, orange, yellow, green, and gray) and sort them based We found the values for our filters by using MATLAB’s
on their color (Fig. 9). To achieve this, we created an image Color Thresholder app. The colored ball filter removed colors
processing pipeline that could extract the pixel coordinates of with low saturation and low values to get rid of the black and
 (a) Image Without Brightness Equalization

(a) Brightness Histogram Without Equalization

(b) Image With Brightness Equalization

Fig. 11: Images With and Without Brightness Equalization

each run. For live tracking of objects, we found it better to use

other methods such as erosion and edge detection for increased
speed.
5) Classifying Circles by Color: Next, we take each of
the identified circles and determine which of the five colors
(b) Brightness Histogram With Equalization (red, orange, yellow, green, or gray) it is closest to. The first
method we tried was finding the average color of the circle
Fig. 10: Brightness Histograms With and Without Equalization and calculating its Euclidean distance to each of the expected
target colors. Then, whichever target color had the shortest
distance would be the color of that circle.
white checkerboard. This left behind only the brightly colored We tried this method in HSV, RGB, and L*a*b* color
balls, but also removed the gray balls. spaces, but none of these produced the results we needed. This
To add the gray balls back in, a second filter band passes method had a lot of trouble differentiating between similar
only the mid-ranged values found in the gray ball. The outputs colors and was not reliable enough to be used.
of both filters combined creates a mask containing all the balls. Instead, we decided to use a different approach that was
Under certain lighting conditions, some regions of the more computationally expensive, but much more reliable. We
checkerboard match the gray values found in the ball which created five different color filters for each of the target colors.
creates artifacts in the masked image. However, these artifacts Each of the color filters is applied to the circles one at a time.
are cleaned up in the next step which is identifying circles. Whichever color filtered circle contains the most pixels is the
4) Identifying Circles: To remove noise from the masked most prevalent color of that circle. We found that this method
image and pick out the colored balls, we used MATLAB’s was the most reliable at differentiating between the different
circle finding algorithm. We tried a variety of different ap- colors.
proaches such as eroding the image and using edge detection,
but we found these methods unreliable. With MATLAB’s F. Object Localization
imfindcircles function we were able to achieve very consistent At this stage in the image pipeline, we’ve obtained the
results even under different lighting conditions. location of each ball and classified its color. There is one
The circle finding algorithm requires more processing power last problem that needs to be addressed to get the location
and so is slower than other methods. However, for our pur- of the ball. When the 2D pixel coordinates of the image are
poses we only needed a single image of the workspace for transformed into 3D world coordinates, the image is projected
directly onto the xy-plane of the checkerboard. However, the
actual ball sits on top of the checkerboard with some fixed
height and is not lying flat on the checkerboard like a piece
of paper (Fig. 12).

(a) Image for Object Localization

(a) Illustration of projection error from side

(b) Object Localization Visualization

Fig. 13: Object localization being run on image after image

processing pipeline, with accompanying visualization.

G. Pick-and-Place

(b) Illustration of projection error from top At this point, we now had all the components we needed for
our pick-and-place system, we just needed to put everything
Fig. 12: Illustrations of projection error from different per- together. First, the arm is moved out of view of the camera
spectives and the camera captures an image of the workspace. The
image is processed using our vision pipeline to pick out the
If this projection error is not accounted for, then the robot colored balls (Section II-E). The camera parameters are used
will attempt to pick up the balls slightly behind where they to project the pixel coordinates of the balls onto the checker-
actually are on the checkerboard. To solve this issue, we board (Section II-C). Then, the checkerboard coordinates are
calculate the actual position of the balls by using similar transformed into robot coordinates (Section II-D). A slight
triangles: adjustment is made for the height of the ball (Section II-F).
Inverse kinematics is used to send the robot to pick up the
h
d=D ball (Section II-B). And finally, the arm drops the ball off
H in the appropriate delivery zone based on its color. With all
Dx these steps combined, we finally had a fully-fledged design
dx = d (13) for picking up and sorting the colored balls.
D
All of the movements were implemented using joint space
Dy cubic trajectories. This was done over task space and/or quintic
dy = d (14)
D trajectories because they were computationally faster. The
Using the x and y offsets from 13 and 14, the actual position added benefits of smoother acceleration or moving the end-
of the object is obtained as seen in Fig. 13. effector in straight lines weren’t necessary for our use cases.
 III. R ESULTS V. C ONCLUSION
Throughout this project, we were able to explore and apply
The methods outlined in this paper resulted in a very a variety of techniques related to robotic manipulators. These
successful pick-and-place robot. In addition to this paper, a topics included forward and inverse kinematics, trajectory
video was made to document the development of our arm, and path generation, differential kinematics, and computer
which can be seen here. vision. We began by solving for forward and inverse kinematic
Our arm was successfully able to separate red, orange, solutions of our arm. We then calibrated our camera using
yellow, green, and gray balls consistently in a variety of MATLAB’s Camera Calibrator app to remove fisheye distor-
different lighting conditions. It was also able to process an tion, allowing for accurate conversion of pixel coordinates to
arbitrary number of balls so long as they fit onto the workspace world coordinates. Next, we used the transformation matrix
with sufficient room for the end-effector motion. between the checkerboard and robot to transform coordinates
Additionally, we were also able to implement live tracking into the frame of the robot. We then developed an image
of moving targets. By using a tweaked version of our vision processing pipeline complete with brightness equalization,
pipeline, we were able to speed up the image processing to distortion correction, image masking, circle detection, object
allow our robot to dynamically track moving balls and pick classification. Lastly, we applied object localization to correct
them up once they stopped moving. for the height of the balls. With everything combined, we were
left with our very own pick-and-place robotic manipulator
IV. D ISCUSSION (Fig. 14).

Completing this project gave us a glimpse into current and

future applications for pick-and-place robotic manipulators.
Robots similar to the OpenMANIPULATOR-X are being used
in a multitude of other fields. For example, there are many
potential uses for such robots in the fields of manufacturing,
packaging, automotive, aerospace, healthcare, and even agri-
culture.
Pick-and-place robots have countless benefits that contribute
to their efficiency in these fields. Most notably, these robots are
capable of increased and continuous productivity. In compari-
son to a human performing manual labor, a robot is much more
efficient and productive. Robots are able to perform actions Fig. 14: Our Pick-and-Place Robot in Action
with increased precision and accuracy. They are programmed
to complete repeatable tasks and are especially practical in R EFERENCES
the fields of electronics manufacturing and pharmaceuticals,
[1] Robotis. OpenMANIPULATOR-X Overview, https://emanual.robotis.
where small components require the utmost accuracy during com/docs/en/platform/openmanipulator x/overview/
assembly. This results in increased product quality and in turn, [2] ELP USB Security Camera Low Light 1080P Sony IMX323 HD Sensor
increased customer satisfaction. H.264 Voice Recording Pinhole Spy Camera For Video Conference.
https://webcamerausb.com
Another benefit of large-scale pick-and-place robotics is [3] J. Denavit and R. S. Hartenberg, “A kinematic notation for lower-pair
their adaptability and versatility. Interfaces for industrial robots mechanisms based on matrices,” 1955.
are made to be relatively simple to program, and because of
this, a different end-effector or different programming allows
for a robot to be easily adapted for the task at hand. A
robotic manipulator designed for assembling small electronic
components could be altered to move and package boxes, and
then altered again to transport fragile chemical equipment.
Increased versatility, accuracy, and reliability result in in-
creased throughput, reduced cycle time, and heightened cost-
efficiency. These aspects are desirable for businesses and com-
panies in every field. The culmination of these characteristics
and the continual improvement of robotic manipulators shows
the increasing desire for automation in a plethora of fields.
Although sorting colored balls is a relatively simple task in
the general sense, working with the OpenMANIPULATOR-X
robotic arm and completing this project helped us learn about
the great benefits of robotic manipulators.