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 [1×1] struct of DH tables for each limb % T_Base [1×1] 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 [3×1] vector a a-parameters from DH table [1×6] vector d d-parameters from DH table [1×5] vector r1 Displacement from shoulder [1×1] scalar fT_W_EE Function handle for EE T-matrix [1×1] 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.
