User Tools

Site Tools


lego_2link_arm

This is an old revision of the document!


LEGO 2-Link Robot Arm Tutorial

Author: Norberto Torres-Reyes Email: [email protected]
Date: Last modified on 03/08/19
Keywords: kinematics, planar, robotic arm, 2 link, lego, NXC, mindstorm

Motivation and Audience

This tutorial is for anyone who wishes to create a 2-link robotic arm using LEGO Mindstorm to reinforce theoretical knowledge about 2-link planar mechanisms. Readers of this tutorial are recommended to have the following background and interests:

*Previous experience with LEGO Mindstorm
*Matlab and C-based programming experience
*Knowledge of linear algebra
*Interests in robotics
*Experience with Solidworks

The rest of the tutorial is presented as follows:

Parts List

LEGO NXT parts list is given in full detail below. Most parts can be obtained from the Mindstorm kit, although some extra parts will be needed from LEGO's Technic line of bricks.

Theoretical Background

The theoretical background needed for this tutorial can be fully covered here. In short, it covers the kinematics of a 2-link arm using using three different methods: Matrix algebra, Geometry, and computational tools. Additionally, it provides an introduction into Matlab simulation of the 2-link arm. The link length values in the tutorial can be easily replaced by the actual link values which are found in the code below.

Build Plans

A few different build plans are available. This includes photographic and 3D model based plans as well as a Solidworks based build plan. The reader may wish recreate the model in both Solidworks and with LEGO parts to familiarize themselves the procedures. The build plans are organized below:





Solidworks Files: solidworkfiles.zip
Assembly Video

NXT Brick Code

'Bricx Command Center' is the software used to program the NXT Brick. The software is C-based and has many different functions for interfacing with the Brick. The code used to program the robot arm can be downloaded below. The code uses the geometric solution of the inverse kinematic equations to solve for the joint angles. A gear ratio for each link is determined first to get proper movement. The link lengths are defined using variables L1 and L2 respectively. Next, the desired end point of the arm is chosen with variables 'px' and 'py' with respect to the base reference system.


// -------------------------------------------------------------------------//
// Inverse Kinematics for 2R planar manipulator with a closed-form solution //
// Created By: Sangsin Park, Ph. D.                                         //
// Modified By: Norberto Torres-Reyes                                       //
// March 07 2019                                                            //
// -------------------------------------------------------------------------//

// Motor's constants
#define JNT1 OUT_A
#define JNT2 OUT_B
#define FULL_SPEED (100)

// Gear's constants
#define gearRatio1 (2)*(56/12)*(-1)
#define gearRatio2 (3)*(-1)

// Links' constants
#define L1 (12)
#define L2 (10.4)

// Global variables
float theta1_ik = 0.0;
float theta2_ik = 0.0;

// Function declarations
bool IK_2R_Planar_closed(float px, float py);

task main()
{
     // button variables
     bool orangeBtnPushed = FALSE;
     bool l_ArrowBtnPushed = FALSE;
     bool r_ArrowBtnPushed = FALSE;
     bool greyBtnPushed = FALSE;

     int cnt_OrangeBtn = 0;
     int cnt_l_ArrowBtn = 0;
     int cnt_r_ArrowBtn = 0;

     // reference joint angles
     float theta1 = 0;
     float theta2 = 0;

     float px =12.0 ;
     float py = 16.0;
     bool IK_ok = FALSE;

     PlayTone(TONE_B3, 05);
     TextOut(0, LCD_LINE1, "Grey BTN Quits");
     TextOut(0, LCD_LINE2, "Orange BTN Home");

     PosRegEnable(JNT1,PID_1,PID_2,PID_3); // Defines PID control values
     PosRegSetMax(JNT1, 1.0*FULL_SPEED, 10); // Set Port A speed limit and acceleration

     PosRegEnable(JNT2,PID_1,PID_2,PID_3); // Define PID control values
     PosRegSetMax(JNT2, 1.0*FULL_SPEED, 5); // Set Port B speed limit acceleration

     while(greyBtnPushed == FALSE)
     {
         greyBtnPushed = ButtonPressed(BTNEXIT, FALSE);
         orangeBtnPushed = ButtonPressed(BTNCENTER, FALSE);
         l_ArrowBtnPushed = ButtonPressed(BTNLEFT, FALSE);
         r_ArrowBtnPushed = ButtonPressed(BTNRIGHT, FALSE);
         cnt_l_ArrowBtn = ButtonCount(BTNLEFT, FALSE);
         cnt_r_ArrowBtn = ButtonCount(BTNRIGHT, FALSE);

         PosRegSetAngle(JNT1, theta1);
         PosRegSetAngle(JNT2, theta2);
         TextOut(0, LCD_LINE4, FormatNum ("JNT 1: %f" , theta1/gearRatio1));
         TextOut(0, LCD_LINE5, FormatNum ("JNT 2: %f" , theta2/gearRatio2));

         // Code here for going to home postion after orange button is pushed //

         if(orangeBtnPushed == TRUE)
         {
             ClearLine(LCD_LINE4);
             ClearLine(LCD_LINE5);
             orangeBtnPushed = FALSE;
             theta1 = 0.0;
             theta2 = 0.0;
          }

         if(r_ArrowBtnPushed == TRUE)
         {
             r_ArrowBtnPushed = FALSE;
             IK_ok = IK_2R_Planar_closed(px, py);
             if(IK_ok == TRUE)
             {
                 theta1 = theta1_ik*gearRatio1;
                 theta2 = theta2_ik*gearRatio2;
                 TextOut(0, LCD_LINE6, "Solution.");
             }
             else
             {
                 TextOut(0, LCD_LINE6, "No Solution.");
             }
          }
     }
}

bool IK_2R_Planar_closed(float px, float py)
{
     float c2 = (px*px + py*py - L1*L1 - L2*L2)/(2*L1*L2);
     float s2 = -sqrt(1 - c2*c2);
     float c1 = (px*(L1 + L2*c2) + py*L2*s2)/(px*px + py*py);
     float s1 = (py*(L1 + L2*c2) - px*L2*s2)/(px*px + py*py);

     if(c2 >= -1 && c2 <= 1 && c1 >= -1 && c1 <= 1)
     {
          theta1_ik = atan2(s1, c1)*(180/PI);
          theta2_ik = atan2(s2, c2)*(180/PI);

          return TRUE;
     }
     else
     {
          return FALSE;
     }
}




2-Link Lego Code

Running, Testing and Analysis

Running the Code

Video of Operation
If the code is compiled and uploaded to the brick successfully, it will be necessary to use a way of measuring the end point coordinates with respect to the base. This is done by attaching a studded base that is parallel to the plane of movement of the arm. Each stud represents 8mm distance and can therefore be used to judge the accuracy of the arm. Each motor has different PID and acceleration parameters that can be tuned heuristically to obtain better arm operation.

Testing and Analysis

The best way to test the arm is to use the studded wall as a reference. When the code is ran, the initial position of the arm is regarded as the zero angle position. With this in mind, it is important to have the arm as level as possible. Testing several points allows you to better see the response of the arm. Friction and slop in the arm will be the biggest factors in accuracy. This is where tuning the parameters will help.

     PosRegEnable(JNT1,PID_1,PID_2,PID_3); // Defines PID control values
     PosRegSetMax(JNT1, 1.0*FULL_SPEED, 10); // Set Port A speed limit and acceleration

     PosRegEnable(JNT2,PID_1,PID_2,PID_3); // Define PID control values
     PosRegSetMax(JNT2, 1.0*FULL_SPEED, 5); // Set Port B speed limit acceleration


PosRegEnable() first defines the port (doesn't have to change). The last 3 parameters are P,I,D parameters respectively.
PosRegSetMax() also defines port, maximum full speed, and max acceleration.
Changing the values will allow you to fine tune the arm.

Conclusions

In conclusion, it was found that it is possible to create a successful 2-link robot arm using LEGO and to control the arm using analytical solutions to the inverse kinematics. It is also possible to tune the arm to achieve better accuracy. Possible future iterations may include improving the mechanical stiffness of the arm to reduce the error in the final position. It is also possible to add a third link as an end-effector for practical uses of the arm. These can include various sensors or grippers. Solidworks can also be used to run simulations or test different iterations of the arm. As long as the parts are properly defined, it is possible to obtain moments of inertia and centers of gravity for each link. This can be used for further dynamic calculations.

lego_2link_arm.1552057846.txt.gz · Last modified: 2019/03/08 07:10 by ntorresreyes