S. Alexander, J Bonham, J Johansson, A. Mewha.
Report 2, 3-D mapping using Lego robotics.
Abstract -- Background information for 3-D mapping project robot. Specifically a high-level design plan, a detailed list of
using a robotic arm built from Lego is presented. This requirements, further details on the progress to date and a list
information includes the literature review and five to 10 of acceptance tests that will enable an evaluation and
references of the research necessary for the project.
II. High Level Design Plan and Implementation Review
As stated in the title, the objective is to build a robotic arm
that can collect samples on an object, and send them to a PC to What follows is a high-level overview of the goals of the
assemble into a 3-D map of that object. To keep costs to a project, and the current design plan. A prototype of the robot
minimum this first prototyping stage is being done with Lego. has also been built. A discussion of the prototype’s current
Lego manufactures central control boxes, motors, gears, rotary implementation follows the high-level overview.
and touch sensors that will be necessary to make the robot.
The central control box uses C programming language.
Unfortunately, this box has connections for only three sensors A. High Level Design Plan
and three motors. This problem was solved by using one The overall goal of the project is to develop an autonomous
motor as an enable switch that could multiplex in more motors robot capable of scanning a 3D object, transmitting that data to
and sensors. a computer and having the computer display a 3D model of
During the course of the project, the group faced and the scanned object. One additional stipulation was that the
solved other problems as they developed. They will only be scanner be able to acquire and display 4th-dimensional data,
introduced here and will be elaborated on completely in the such as the temperature of the object at different points on the
appropriate sections: surface.
1. Some alternatives to touch were considered for
The robot itself is to be constructed of Lego. Specifically, it
sampling the object, such as lasers or sound. However, those
will be constructed using a Lego Mindstorms kit. A
required faster data processing and more motors and/or
Mindstorms kit is a collection of Lego pieces that is
sensors then the brick could provide. The operating budget
specifically tailored to building mechanical devices. It
was also a factor. contains motors, gears, and sensors; all of which are controlled
2. In order to provide the precision control and the by a programmable microcontroller called a Brick. The
torque necessary to move the large robotic arm in small modularity and flexibility of Lego are key factors which allow
regular steps, all of the motors needed to be geared down. for a very easy construction process and easily allow new
This took some torque calculations and careful selection from ideas to be implemented and tested.
limited available parts.
3. Backlash was also considered, that being when the There are a variety of different approaches available to do 3D
gears do not perfectly mesh and some movement is possible scanning, ranging from using complex laser methods to taking
without a rotation, and this is being monitored and calibrated and processing pictures of the object. The method chosen for
for in the software of the motor controls. this project was a mechanical approach. Mechanically
4. Other challenges forced the robotic structure to scanning means the surface of the object is physically traced
evolve from fixed base with a rotating shoulder to a rolling by a mechanical probe. The reason for this was twofold. First,
base with a shoulder, elbow, wrist, and a touch sensor as a it was the simplest method to implement as it requires very
fingertip. Most recently the touch sensor and motor controlled simple software calculations in order to produce surface point
wrist was replaced with a position tracked free rolling wrist measurements. Second, the robot was to be build with as much
and a wheel for a finger. Position sensors also track the Lego parts as possible. Lego lends itself very well towards
shoulder and elbow angle of the arm, which allows for a mechanical implementations and contains many ready-made
mechanical parts. In addition, its ease of use and flexibility
calculation of the exact position of the arm's finger.
aids in quickly visualizing and building prototype designs.
All data is sent back to the PC, via an infrared device on
Several methods of physically tracing the surface of the object
the USB port, to be interpreted into a 3-D map of the object. were investigated. In the end, the design chosen was to have a
Additional information such as temperature can also be 3-joint arm attached to a fixed base, which would roll
gathered an illustrated on the object using color encoding. alongside the object to be scanned. The scanner would slowly
Organizing the raw data into a 3 or 4 D image is done in walk alongside the object, stopping every few millimeters to
Matlab using a point cloud method that will be described in extend the arm and trace the contour of the object. This
more detail later. method was chosen primarily because of its simplistic nature;
As an outline, what follows is more detail on the however, using this method provided one addition benefit.
When scanning, all the data points are collected row-by-row. Currently a prototype of the robot has been constructed. This
This allows the surface of the 3D object to be recreated in a prototype closely follows the design laid out in the high-level
similar row-by-row fashion, effectively eliminating one of the design section.
major complexities with more advanced scanning methods
where the object is scanned piecemeal, and then reconstructed The underlying layout for the design is based on a robot by
later on. Figure 1 shows gives a visual representation of how a Dave Baum , as found in his book “Definitive Guide to
scan is done. The robot itself, (yellow), moves alongside the Lego Mindstorms”. His design, which consists of a rotating
object in small steps in the direction of the green arrow. The base with a two-joint arm, was used as the starting point for
robot arm, (blue), extends in the direction of the red arrow and the rest of the implementation.
scans the object’s surface.
The rotating base was removed and replaced with a fixed base;
since the robot will be moving alongside the object no rotation
was needed. Wheels were then added to the base to allow the
robot to move alongside the object being scanned. In addition,
the two-joint arm was expanded and reworked into a 3-joint
arm, as required by our design plan.
Custom software was design and written for the Lego Brick.
The software currently has no methods to collect and send data
back to the computer, but it can perform all other tasks
required to perform a scan. The software uses simple control
algorithms to ensure the smoothest scan possible. It is
currently operated by using the built-in buttons on the Lego
Figure 1 General overview of scanning procedure III. Requirements Section
Finally, Matlab was chosen to render the 3D Models, as it This project is composed of five main deliverables
provides simple, effective tools to create and display 3D 1- Mechanical system
models. It also provides tools to display 4 th-dimensional data 2- Programming to Operate the Mechanical System.
as a color map on top of the object, an important part of the 3- Data Transmission from the RCX to the Computer
project. An example of a color map is shown below in figure 4- Interpreting and Displaying of the 3D data on the
2. The object in the figure is colored according to height. The Computer
highest areas are colored red, while the lowest areas are 5- Reading and Displaying Fourth Dimensional Data,
colored blue, however, height need not be used as the coloring Temperature.
method. Temperature, moisture, hardness, and many other
measurements may be used as a source for 4th-dimensional The following is a breakdown of each of the four deliverables
data. into their internal operations.
A. Mechanical System
The mechanical system must be able to pass a sensor over
each part of an object which is being scanned. This will be
achieved by using a 3 jointed arm and a mobile base which
supports the arm. The base of the robot will travel in the Z
direction, and at each position along the Z axis of the object to
be scanned the arm will pass over the object reading back the
X and Y information about the object. Data about the current
position of each rotating joint will be read back using rotary
encoders. Motors will be used on the base, the shoulder joint
of the arm and the elbow joint of the arm. The Wrist joint of
the arm will be left free to rotate so that during the scan it will
accurately record data about the surface of the object being
Fig 2 An example of a color map
B. Software to Operate the Mechanical System.
B. Implementation Review
The software to operate the mechanical system will interface
to the 3 motors and 4 rotary sensors on the mechanical system.
Using the motors to move the base of the robot, shoulder joint load further along the axel. The most significant change is we
of the arm and elbow joint of the arm and the rotary encoders added a crown gear to improve coupling of the shoulder axel
to read back their current positions the software will be able to and worm gear on the motor shaft. The base has undergone
move the robot to any arbitrary position. The main purpose of some major changes from the original design. We have
the software is to use the ability to move the robot to any removed the rotating component of the base and replaced it
arbitrary position in order to pass a sensor over a large with a wheelbase driven by a single motor. The gearing on the
percentage of the object under scan. By passing the sensor drive motor for the base has been arbitrarily chosen to be
over the object being scanned, data collected from the four 8:24:40. The effectiveness of this gearing to produce a
rotary encoders during the scan can be used after decoding to suitably small step size; will be determined once we are able
create a representation of the object on the computer. to start reading data points and testing the visualization
software. Please see Figure 4 which demonstrates the final
C. Data Transmission from the RCX to the Computer base, shoulder carriage, distal component of the shoulder joint
The data collected from the mechanical system and the and Figure 3 ,all three combined to form the base/shoulder
software will not be used directly by the RCX. The data will joint.
be sent to the computer using the infrared port which is
usually used to program the RCX.
D. Interpreting and Displaying of the 3D data on the
The data transmitted from the RCX to the computer is in raw
format, it must be interpreted by the computer in order to
extract the correct x, y, z data. This interpretation will be
performed using Matlab. The transformed data will then be
displayed using Matlab as well. This Displayed data will
ultimately be a three dimensional representation of the object
which was scanned.
E. Reading and Displaying Fourth Dimensional Data,
Additionally, the scanner will be able to record a fourth piece
of information about the object which is being scanned. This
will be the temperature of the object at each point measured.
This data will be displayed on the computer as a different
color on the three dimensional representation, where red will
be the hottest temperature and blue the lowest.
Significant progress has been made on all aspects of the
project. Most significantly, the model design has evolved to a
point where we will be able to begin testing the other
components of the project such as the software and data
acquisition. The following section will deal with the progress
made on 1) Robot design, and 2) Control Software design.
A. General Robot Design
i) Shoulder joint and base
The shoulder joint has remained relatively unchanged from the
design of Dave Baum. The joint is sound and serves our
purpose well. The gearing is effective and currently produces
enough torque for our requirements. The current gearing is
40:8 at the shoulder joint and 30:1 between the motor and
shoulder axel. The use of two 40 tooth gears along the
shoulder joint axel distributes the force over a larger area and
effectively reduces the amount of bending of the axel. This
may present a problem if the load is increased substantially. It
Figure 3 (Top to Bottom) i) Shoulder carriage ii) Distal shoulder joint iii)
may become necessary to calculate internal joint normal and Base with shoulder and base motors.
reaction forces. To compensate we may have to distribute the
Design change 1 involved replacement of the standard 24
tooth gear with a crown gear. The gear change was to remove
some of the gear slap inherent to the original design. After
initial testing there appeared to be significant lateral mobility
due to the sole lift-arm support for the forearm. This was
removed and replaced with two supports on each lateral aspect
of the joint. This necessitated the removal of one axel and the
two joining gears that were initially present on the lateral
aspect of the joint. This change allowed the use of one axel
Figure 4: All three components from Figure 1 combined to form the
Base/Shoulder joint complex. which could be moved more distally on the arm. The use of
one axel allowed us to center the crown gear with the worm
ii) Elbow joint gear which improved the meshing and reduced gear slap even
The initial design for the elbow joint once again was inspired further (Figure 6).
by Dave Baum. The joint has undergone some significant
changes to better adapt it to our purpose. The joint as per Mr.
Baum was initially designed for grasping objects between two
opposable members. The design had two axels which rotated
in opposite directions. The gearing included a regular 24 tooth
gear offset to one side meshed with a worm gear. The two
axels were attached via two 8 tooth gears located on the lateral
aspect of the joint. The initial support for the forearm was
provided by a single 9 hole lift-arm (Figure 5).
Figure 6: Elbow with bilateral lift-arm support and centered crown gear
Design change 2 became apparent once the joint was actually
Figure 5: Side Side (top) and bottom (lower) view of original elbow joint with
moved via the 9V Lego motor. With any significant velocity
second yellow lift-arm removed or at the extremes of movement the joint support structure
would break apart. The support box enclosing the elbow
gearing was reinforced which to date has provided enough
support. This may have to be reinforced further once more sensor. The rotation sensor is currently fixed to the wrist axel
stress is applied to the joint while the robot is actually directly.
scanning. (Figure 7).
Figure 9: Final wrist joint design
Figure 7: Final to date design of elbow joint, with reinforced gear housing.
B. Control Software Design
In order to interface with the Lego microcontroller a Linux
emulator for Windows was required. The emulator we used,
iii) Wrist joint
which was suggested by the authors of BrickOS , was
The initial wrist joint was essentially a duplicate of the elbow
Cygwin. Cygwin is freeware provided by . We are
joint with a shorter overall length. A touch sensor was
attached to the most distal aspect of the final member. currently using BrickOS as a compiler. BrickOS allows the
Initially the design was intended to have the joint angles use of C as a programming language, making the Lego
microcontroller quite an effective tool and allowing us to
calculated as the touch sensor was pressed. The robot would
exploit all of its capabilities. We have broken the code down
move to a start position. The arm would then retract a fixed
into different beta versions as we progressed. These are listed
amount and lower until the touch sensor was activated
below with a brief description of the changes made at each
signaling the arm to stop. Joint angles would be calculated
and the position of the end effector known giving a position in stage.
3D Cartesian coordinates. Figure 8 demonstrates the original
wrist joint designed to accomplish this task.
- Turned the motors on/off and varied the speed.
- The rotary encoders were used as an input and
controlled the motor of the shoulder joint. The stop
position of the joint (motor) to go to a hardwired
position based on the encoders’ input.
- A proportional controller was implemented. This
enabled us to hardwire a position based on encoder
input and have the motor speed slow as the position
Figure 8: Initial wrist joint design Version 1.4
- Incorporated the proportional controller to other
Upon consulting Dr. Macnab with respect to our design, he joints. Set limits for scanning and incorporated a for
suggested an alternate approach. To have the end effector free loop which tells the arm to go to discrete positions.
floating and calculate joint angles at specified intervals as the
arm retracted across the object being scanned. The group
decided this was a good suggestion and has modified the wrist
joint accordingly. This approach will simplify the overall Version 1.5
design considerably and save one output port as a motor will - Incorporated a feedback-loop which tells the
not be required to power the wrist joint. Figure 9 elbow to follow the progression of the shoulder
demonstrates the new and final to date version of the wrist automatically.
joint. This design should allow for early testing of data
acquisition. One further possible change before testing is to
add some gearing to improve the accuracy of the rotation
(all = 0)
Wait for arm to be
put in cal pos.
Displ “Cal Pres
!Pressed Calc Motor gain Calc Motor gain
_view Sh_up, Elb_dwn Sh_dwn, Elb_up
Motor_spd * gain Motor_spd * gain
Elbow at Elbow at
Data_cnt = 0
! Reset arm
Go to Go to
If elb not Figure 10: Flowchart for version 1.5 of code implementing the PD controller
at with gain
The current mechanical design and version 1.5 of the control
software will allow us to start the next phase of testing, which
is data acquisition. We plan on beginning these tests in early
Cal_pos V. Acceptance tests for the deliverables
The overall deliverable of the project is a robot which will be
able to autonomously scan an object and create a three
dimensional representation of the object on a computer screen.
For this overall design goal to be achieved an accuracy of 2.5
diamond mm must be achieved on the overall scan. The following
acceptance tests relate to the specific deliverables of the
A. Mechanical System
The first acceptance test for the mechanical system is that the
base be able to move linearly along the object being scanned.
It must be controllable so that it can be stopped or moved to
any position along its accepted range of travel. The shoulder
of the arm must be controllable so that it can be positioned
anywhere from horizontal to the ground to nearly vertical
(limited by the mechanics of the shoulder). The elbow of the
arm should be controllable so that it can be positioned
anywhere from parallel with the shoulder when fully extended A. Data Transmission
to nearly parallel with the shoulder again when fully retracted. Performing a scan with the prototype is currently impossible.
The wrist must be able to freely move fully about its axis The portions dealing with data transmission are not yet
(until it contacts the elbow). The sensors used on each joint complete. In order to accomplish this, the LegOS Networking
must have an accuracy which by way of gear reduction will be Protocol (LNP) will be utilized. LNP provides basic input /
able to achieve the design goal of 2.5mm scanning accuracy. output functionality and error correction which will allow the
Lego Brick to communicate with a computer or another Lego
B. Software to Operate the Mechanical System. Brick.
The acceptance tests for the software are incremental in nature
allowing for evolution of the software over time. The first Two steps are required to bring the data from the robot into
acceptance test for the software is that it must be able to Matlab for visualization. First is the data must be sent to the
operate the 3 motors used in the mechanical design. Next the computer. This will be done using a simple serial port or USB
software must be able to read back data from the four rotation connection. From there, the data must be processed and put in
sensors. Third, the data from the rotation sensors must be able a Matlab friendly format. This will likely be done by arranging
to be used to add control so that the different joints can be the data into tables that Matlab can directly load into a matrix.
commanded to go to specific positions. The software must
then be able to control multiple motors and sensors Both of these steps can be accomplished by writing one a
simultaneously. Fourth, the software must be able to tell the simple computer program. The program would read in the data
mechanics to make multiple scanning passes over the object from a scan, process the points and arrange the data in matrix
being scanned. Finally the software must be able to record the form, and then finally save the data to a text-based data file.
data from the rotation sensors during the scanning passes. From there, a simple Matlab script could open up the data file
and read in all the values into a matrix. Visualization would
then only be a few commands away.
C. Data Transmission from the RCX to the Computer
The data must be able to be sent via the infrared port to the
computer, it must take no more than 1 second of transmitting B. Reducing Lego Flex and Gear Slack
for each scanning pass of the arm. The data transferred must Currently there is a significant amount of flex in the Lego
have zero corrupted bits, error checking must be implemented structural members, as well as a fairly large amount of gear-
to avoid erroneous data transfer. slack in the gear connections. Since a scan cannot be done
with the current prototype, there is no way to know if these
D. Interpreting and Displaying of the 3D data on the factors will noticeably affect the accuracy of a scan. However,
Computer steps will be taken anyway to mitigate the possible effects on
The computer must be able to convert the raw data correctly accuracy of these problems.
into x, y, z components. This conversion will be verified by
measuring the components directly from the arm using a ruler Flex in the Lego members is intrinsic due to the plastic
the accuracy must be within 2.5 mm. The display of the data construction and cannot be done away with completely.
will be acceptable if the object displayed resembles the object However, proper structural reinforcement can lead to very
scanned. Certain inaccuracies may have to be disregarded as strong designs. Using cross bracing, thicker members, and
unavoidable. Such inaccuracies may include bending of the even superglue, the effects of flex can be mitigated. These
frame of the robot, inherent inaccuracy in the rotation sensors improvements are currently being evaluated, and will likely be
and gear slap. implemented once the design has been finalized as some of
them, notably the superglue, can have very lasting effects
E. Reading and Displaying Fourth Dimensional Data, which are difficult to undo.
The temperature data must be able to be read into the RCX, Gear slack between the gears is caused when the gears don’t
this will be verified by displaying the raw temperature data on mesh perfectly and there is a small space between the teeth
the screen of the RCX ensuring that the change in temperature which can cause subtle movements in the gear system. The
is being read correctly. The display of the temperature data on nature of the Lego gear design prevents a truly competent
the three dimensional model will be verified by creating a hot solution. All that can be done is to try different gear
zone on the object being scanned and ensuring that it is arrangements and use the one which produces the least gear
correctly displayed on the model. slack.
VI. Future Design Changes and Improvements C. 4th-Dimensional Data
4th-Dimensional data is an important part of the design
specification. At the moment, no design decisions have been
Following is a brief overview of improvements and additions made regarding 4th-dimensional data scanners. Current
which are currently being considered for implementation. thinking would add a special mount for a scanner on the end
of the 3-joint arm. This scanner could be interchanged with
other scanners, allowing temperature, moisture, hardness, or
any other type of data to be collected relatively painlessly.
Once the scanner is connected and operational the 4 th-
dimensional data would be sent back with the regular scan
measurements to be processed later in Matlab.
D. Improving Accuracy
The current prototype is incapable of performing a scan, so it
is unknown at this time just how accurate the scan is. Already,
however, there are several areas where improvements can be
Foremost of these areas is the rotation sensors used throughout
the robot. The sensors being used only indicate a change once
every 22º of rotation. Such a large range is clearly not
acceptable for scanning purposes since it translates into 22º of
free range motion with no change being noticed. To combat
this situation gear-up is used. The encoder is connected to the
shaft using paired gears in such a way that 1 rotation of the
shaft may equal n rotations of the encoder. This has the effect
of reducing the rotation range from 22º to 22º / n. This means
that every 22º / n degrees of rotation in the shaft causes a
change in the rotation encoder, which improves rotation
accuracy significantly. The actual ratio of the gears, n, is
arbitrary and will be chosen based on what works best.
Another area of concern is the horizontal tracking of the robot
alongside the object being scanned. Currently the prototype
uses a set of wheels to move the base horizontally. The initial
plan had the robot rolling along two parallel sets of toothed
tracks, using gears instead of wheels. However, wheels were
implemented because of a shortage of toothed track sections.
For a proper scan the horizontal tracking step might be as
small as 1 mm. Wheels do not provide the accuracy needed to
achieve such a small step size. As such, additional toothed
track pieces were placed on order. Once the additional pieces
arrive, the drive system will be changed over to use the track
instead of the wheels.
D. Baum, "Definitive Guide to Lego Mindstorms",2nd ed., New York
a! Press, 2003 pp 71-81 and Ch 19.
Markus L. Noga “brickOS™” http://brickos.sourceforge.net/
Cygwin, open source, http://cygwin.com