MEC4456 / TRC4800 Robotics Lab 1
Direct Kinematics and Inverse Kinematics using
simulated Bioloid Humanoid Robot
In this laboratory class, you will be given access to a Simulated Robotis Bioloid Premium
humanoid robot via MATLAB with which you will implement and examine the theoretical
knowledge you have gained so far in Robotics MEC4456 / TRC4800. During this class you
will be required to perform a number of tasks with your robot and have them checked by a
tutor. All required tasks to be completed are displayed in bold typeset. After this lab you
will also be required to submit a report on your findings, details of which are found at the end
of this document. Please note that all MATLAB files you are required to edit are found in the
‘functions’ folder.
Robotics Bioloid Premium Humanoid Robot
The Bioloid Premium Type A robot is an 18 degree of
freedom humanoid robot designed to be used by hobbyists
and in a teaching environment. The joints and limbs are a
simplified replica of the human body and as such this
robot's movement and joint space is similar to that of a
human. Each limb is operated by a Dynamixel AX-12A
Servo-actuators which control the movement of each joint.
These precision DC motors deliver a stall torque of 1.5Nm
and can rotate at up to 59rpm. Each motor has features
such as control circuitry; network functionality with 1 MBps
communication speed; full feedback on Position (300°),
speed, DC current, voltage and temperature; voltage, DC
current and temperature automatic shutdown and are high
torque servos.

These motors are controlled by a Dynamixel CM-530 controller, onto which programs can be
downloaded and executed from the six buttons on the front or a separate remote controller.
The robot can be powered either through a power cord running through an adapter to a 240V
wall socket or from the LiPo 11.1v LBS-10 battery pack supplied with the kit.

The CM-530 controller has the ability to send and receive information from the AX-12A servos;
the controller sends commands to the servos with the use of instruction information packets.
Dynamixel does the Asynchronous Serial Communication with 8 bit, 1 Stop bit, and None
Parity.


Part 1: Base Frame Definitions
Before we can begin actuating our robot, we must assign frames to the mechanism we are
trying to control. Frames are attached to each joint of the mechanism which gives us the ability
to fully define the mechanism in 3-dimensional space using matrix arithmetic.

The notation of each frame consists of two numbers: the limb ID, and the frame number along
that limb,such that {limb ID, frame number}. The orientation of each frame is represented by
the color of {Red, Green, Blue} for the {X, Y, Z} axis. The limb IDs are:
1. Upper left
2. Upper right
3. Lower left
4. Lower right
Therefore, frame {1,0} is the base frame for the upper left limb, frame {1,1} is the next frame
along the same limb, {2,4} is the fourth frame (end effector frame) of the upper right limb and
so on. Figure 1 shows the location and orientation of all the frames on the Bioloid robot, and
all dimensions associated with it.

Figure 1. Robot frames and dimensions.
Each limb has a base frame {L,0}, where L is the limb ID, defined relative to the body frame
{B}. This indicates the locations of the shoulders, T_B_{1,0} and T_B_{2,0}, and hip joints
T_B_{3,0} and T_B_{4,0} on the body respectively.

Task 1.1) Define the transformation matrices of frames {1,0}, {2,0}, {3,0} and {4,0},
relative to the body frame {B}. Tip: The rotational portion of your transformation matrices
should be obtained from Figure 1 by inspection, and the translational component represented
by their algebraic values, i.e. a_i and d_i. Where only the x and z-axes are shown, use the
right hand rule to complete the y-axis.


T_B_{1,0} =



T_B_{2,0} =



T_B_{3,0} =



T_B_{4,0} =


Part 2: Frame Definitions via. Denavit-Hartenberg Parameters
Now that the base frames for all limbs are defined, the rest of the frames can be assigned.
Normally, the z-axes of each revolute or prismatic actuator is assigned first before defining the
origin of each frame, then x-axes of each frame. However, we have already assigned these
frames for your convenience. To define transformation matrices for each of these frames, we
assign Denavit-Hartenberg parameters for each frame, propagating from the base of each
limb to the end-effector. In this unit, the method defined by Craig in Introduction to Robotics:
Mechanics and Control is used to fill out the tables, where the DH parameters are:

● ai-1 – the distance from Zi-1 to Zi measured along Xi-1
● αi-1 – the angle from Zi-1 to Zi measured along Xi-1
● di – the distance from Xi-1 to Xi measured along Zi
● θi – the angle from Xi-1 to Xi measured along Zi

Task 2.1) Fill out the Denavit-Hartenberg tables, using the parameters specified. Tip:
Ensure that the directionality of rotations are followed. Also include any offsets to \theta that
are applied in the home position shown in Figure 1 (left and right legs). Again, use a_i and
d_i parameters to represent translational offsets.

Table 1: DH Parameters for the Left Arm (1)
i ai-1 αi-1 di θi
1.1
1.2
1.3
1.4

Table 2: DH Parameters for the Right Arm (2)
i ai-1 αi-1 di θi
2.1
2.2
2.3
2.4

Table 3: DH Parameters for the Left Leg (3)
i ai-1 αi-1 di θi
3.1
3.2
3.3
3.4
3.5
3.6
3.7

Table 4: DH Parameters for the Right Leg (4)
i ai-1 αi-1 di θi
4.1
4.2
4.3
4.4
4.5
4.6
4.7
Part 3: MATLAB: DH_Bioloid.m
Now that the base frames and DH parameters are defined, we can input these answers into
MATLAB to start the modelling process. In the functions folder, there is a file called
DH_Bioloid.m. This contains the skeleton code that defines the base frames and DH
parameters for the robot. Please observe the following code segment.

%DH_BIOLOID DH table for the Dynamixel Bioloid robot
%Output
% DH_Table [1x1] struct of DH tables for each limb
% T_Base [1x1] struct of T-matrices representing the location of
% each limb base relative to body {B}
% Parameters DH parameter a and d values (in metres)

d = [ 72 22 42.5 38.75 120.75 ]*1e-3;
a = [ 11.5 68 106 76.98 76.94 33 ]*1e-3;

%% Limb Upper Left
T_Base.Limb_Upper_Left = [
% ============= YOUR CODE (START) ================
% Fill matrix here, for example
% 1 0 0 0;
% 0 1 0 0;
% 0 0 1 0;
% 0 0 0 1
% ============== YOUR CODE (END) =================
];

th1 = sym('theta_1', [1 3], 'real');
DH_Table.Limb_Upper_Left = [
% ============= YOUR CODE (START) ================
% Fill matrix here, for example
% 0 0 0 th(1);
% pi/2 a(2) d(2) th(2);
% 0 0 0 th(3)+pi/2;
% 0 0 0 pi
% ============== YOUR CODE (END) =================
];

As observed, the a and d variables are defined for you, so these should be used in your base
transformation matrices and DH table matrices. The variables T_Base and DH_Table are
structured variables, and their fields are self-explanatory. These are the output variables for
this m-file. The theta variables for this limb are defined as an array th1. This means th1(1)
is \theta for the first actuator, th1(2) for the second actuator and so on.

Task 3.1) Fill out the empty matrices in DH_Bioloid.m as indicated by the comments.
Part 4: Direct Kinematics
Direct kinematics allows for the calculation of the position and orientation of an end effector
with respect to a specified frame, normally a base frame. With respect to this robot, we are
interested in the location of the hands of the left and right arm with respect to the global base
frame defined in Figure 1. This is achieved through the use of transformation matrices, which
can be defined from frame to frame, i.e. from Fi-1 to Fi, using the DH parameters. The general
transformation matrix which transforms a point in Fi into Fi-1 is:

This equation will generate the incremental transformation matrices for each limb, from the
base to the end effector. This is necessary to find the end effector position relative to any
other reference frame (world {0} or body{B}), which is what direct kinematics is about.

The m-file DH2Transform.m calculates the direct kinematics based on the given base
transformation matrices and DH tables given in DH_Bioloid.m. You will complete
DH2Transform.m such that each individual link transformation matrix is calculated.

Task 4.1) Complete the skeleton code such that all individual link transformation
matrices are solved. Tip: In the for-loop for generating T_B_, ensure that the first frame
(i=1) is multiplied by the limb base frame. Otherwise, your limbs will not be situated correctly
relative to the body!
Part 5: Testing the Bioloid Model
From this part onwards, we will work with the Bioloid virtual model in MATLAB. Please refer
to Bioloid Model Documentation on how to initialise this model. The documentation also
describes all the relevant functions required to complete all laboratory tasks. Once you have
initialised the model, your robot should look like Figure 2.

If it does not, make changes to the DH2Transform.m or DH_Bioloid.m files, then run
L.Bioloid.DHUpdate() to update the forward kinematics equations. If it results in an error,
then the model needs to be re-initialised instead. See the on how to initialise the model.

If your model looks correct, then we can move onto the next step of testing the direct
kinematics.

Tip: Included solutions.
You may have noticed there are lines commented out in the code that imply solutions can be
loaded. This may look like this:
%% Uncomment to use given solution for debug purposes
% [DH_Table, T_Base, Parameters] = DH_Bioloid_soln;



Figure 2. Bioloid Robot in its home position and default view.

If you uncomment the lines containing solution functions, then the built-in solutions for the
Bioloid model will be used instead, which will load the model in its correct form. These
functions are given to you for your own reference and debugging purposes only. Using these
functions will return rather intrusive warnings in your command window to discourage you from
using them. It should be obvious that these functions should not be used in your submissions,
and will result in loss of all marks for that part if found to be uncommented.

Task 5.1) Calculate the pose (transformation matrices) of the left and right hands for
the joint values outlined in Table 3. Tip: You can use the user interface, or use the
ServoSet() function.

Table 3: Joint angles for Task 5.1
θ2,1=(2,1) θ2,2=(2,2) θ2,3=(2,3) θ1,1=(1,1) θ1,2=(1,2) θ1,3=(1,3)
Joint Angles (Degrees) 50 -65 -45 -60 30 100

Task 5.2) Describe the robot’s pose. Attach an image of your robot in the report.

Part 6: Arm Inverse Kinematics
Inverse kinematics calculates the actuator angles required to achieve a particular end-effector
pose. This is a very important and commonly-used aspect of robotics as it allows for path
planning in the task space, where the workspace can be easily visualised. Your task is to
formulate, and program the inverse kinematics model for the Bioloid 3-DoF arms. The code
for the inverse kinematics model is contained in a file called IK_Bioloid_Arm.m in the
functions folder. It is missing key equations required to calculate the IK for the Bioloid
arms. The five inputs required to calculate the IK will always be given at the call of the IK
function under ik_arm(), where you need to fill out your IK equations:

Input Description Size
p End effector position [3x1] vector
a a-parameters from DH table [1x6] vector
d d-parameters from DH table [1x5] vector
r1 Displacement from shoulder [1x1] scalar
fT_W_EE Function handle for EE T-matrix [1x1] function handle

Further documentation can be found in this m-file and in the Model Manual. The function
handle fT_W_EE returns a transformation matrix when given a joint space pose, i.e.

T = fT_W_EE([0 0 0]) % T-matrix of the end-effector at home position
T = fT_W_EE([0 0 pi/2]) % T-matrix of the end-effector with elbow bent
% 90 deg

Task 6.1) Derive the inverse kinematic solution of the humanoid robots left and right
arm, and enter these equations into IK_Bioloid_Arm.m. Tip: Your equations should
depend only on position, and parameters a and d from the DH table. Also, your solutions to
the left and right arm will be similar, but not identical, hence arm ID is given to differentiate
between solutions.

Similar to the forward kinematics, you can also use the IK solution function for reference by
commenting out the relevant line. However, again warnings will be given in the command
window when you use this function.

Task 6.2) Using the EESet() function, move the end-effectors to the following positions
(relative to world frame) listed below. What body parts is the robot touching? Provide
the joint solutions for these poses. Tip: If your inverse kinematic solutions are correct, then
the robot’s end effectors should be very close to the surface of the robot. If you are in the
default camera view, use the “Orbit” camera mode to get a clearer view of limb 2.

Limb 1: [0.00 0.00 0.05] Limb 2: [0.05 -0.07 -0.20]

Task 6.3) Move the arms to the positions defined in Task 6.2 by changing the X, Y and
Z coordinates sequentially in the User Interface. Give reasons to any issues arising
from using this method. Tip: A broken UI is not a valid reason!

Lab Report Requirements.
A formal lab report (i.e. Aim, Method, Results, Conclusion) is not required. Your report simply
needs to have addressed the bolded tasks outlined throughout the lab instructions. A summary
is as follows;

● Task 1.1) Define the transformation matrices of frames {1,0}, {2,0}, {3,0} and {4,0},
relative to the body frame {B}.
● Task 2.1) Fill out the Denavit-Hartenberg tables, using the parameters specified.
● Task 3.1) Fill out the empty matrices in DH_Bioloid.m as indicated by the
comments.
● Task 4.1) Complete the skeleton code such that all individual link transformation
matrices are solved.
● Task 5.1) Calculate the pose of the left and right hands for the joint values
outlined in Table 3.
● Task 5.2) Describe the robot’s pose. Attach an image of your robot in the report.
● Task 6.1) Derive the inverse kinematic solution of the humanoid robots left and
right arm, and enter these equations into IK_Bioloid_Arm.m.
● Task 6.2) Using the EESet() function, move the end-effectors to the following
positions (relative to world frame) listed below. What body parts is the robot
touching? Provide the joint solutions for these poses.
● Task 6.3) Move the arms to the positions defined in Task 6.2 by changing the X,
Y and Z coordinates sequentially in the User Interface. Give reasons to any
issues arising from using this method.

Please also submit the following edited MATLAB Files, all files are found in the ‘functions’
folder;
● DH_Bioloid.m
● DH2Transform.m
● IK_Bioloid_Arm.m

Your final submission should include a pdf of your lab report and any requested matlab files.
Please refer to moodle for submission details including date and location.