[WIP] Baby's first mindstorms project - 3-axis robot arm

[jamesaguilar]

I always lusted after mindstorms as a kid, but I never had it. I don't know why, because my parents got me various things. Maybe I just didn't ask? Anyway, now I'm an adult who has his own money, so I can have things like this. It's so exciting! I have scarcely built anything made out of Legos in twenty years, and it's truly a rush to be back.

What I've built so far: a 3 axis robot arm with a 360 degree rotating base. I only have the ev3 set, so I don't have enough pieces to make anything really robust. Particularly, the arm is far too flimsy to hold a manipulator. 

At the moment I have a program to learn the throw of the upper two motors. The turntable can rotate 360 degrees, and I think I probably won't bother to write a zero function for it. I have worked out the math for the forward kinematics, at least in abstract. I have to measure the model to see where we actually are. I had initially intended to implement the forward kinematics locally. But then I tried to learn the SQP algorithm and my brain melted. I do not know if the EV3 would be powerful enough to run it.

My new plan is to implement a very simple control server on the ev3 that will tell its position and receive commands to move. My desktop will run the inverse kinematics using the prebuilt SQP implementation in scipy.

My next intention is to get one of the prior-year FLL kits and build a robot that can complete the tasks. I'm too old by far to play in these leagues now, but solving the challenges will be a fun project in its own regard.

By the way, building this was really freaking hard, and I'm pretty sure there are a lot of illegal constructions in it! I was already impressed with the people who were skilled at modeling things in Lego, but now that I've tried building something myself, my level of amazement has at least doubled!

[Mr Jos]

3 hours ago, jamesaguilar said:

...

At the moment I have a program to learn the throw of the upper two motors. The turntable can rotate 360 degrees, and I think I probably won't bother to write a zero function for it. I have worked out the math for the forward kinematics, at least in abstract. I have to measure the model to see where we actually are. I had initially intended to implement the forward kinematics locally. But then I tried to learn the SQP algorithm and my brain melted. I do not know if the EV3 would be powerful enough to run it.

My new plan is to implement a very simple control server on the ev3 that will tell its position and receive commands to move. My desktop will run the inverse kinematics using the prebuilt SQP implementation in scipy.

My next intention is to get one of the prior-year FLL kits and build a robot that can complete the tasks. I'm too old by far to play in these leagues now, but solving the challenges will be a fun project in its own regard.

 

By the way, building this was really freaking hard, and I'm pretty sure there are a lot of illegal constructions in it! I was already impressed with the people who were skilled at modeling things in Lego, but now that I've tried building something myself, my level of amazement has at least doubled!

• That would be your starting point, adding all lengths between every joint to variables. This will allow good kinematics.
• Then adding some way to "zero" all motors at the start of the program is very, very, very veeeeryyy usefull. Without it you can not make an accurate robot. Just some .run_until_stalled(...) can do wonders already.
• I'ld say yes, the EV3 should be powerfull enough, as I've managed to get full Forward AND Inverse Kinematics working on the EV3 only without the use of a desktop. And this for a 6-axis robot, so a 3-axis should be no trouble.

When saving a waypoint, the Forward Kinematics are used to read the current XYZ/Roll/Pitch/Yaw position. Inverse kinematics are constantly used to create a straigth path between the waypoints.

• I found that the best way to write out these programs, is first make some sketches with old A4 paper and a pen how all joints work.

And I don't recommend to start writing out the Forward Kinematics for a 6axis at all.. this is all the Sin/Cos math just to find XYZ..

But a 3-axis should be wayyy more easy.

Anyway, enjoy your future projects with your EV3!

Edited August 31, 2023 by Mr Jos

[glowytheglowbug]

35 minutes ago, Mr Jos said:

• That would be your starting point, adding all lengths between every joint to variables. This will allow good kinematics.
• Then adding some way to "zero" all motors at the start of the program is very, very, very veeeeryyy usefull. Without it you can not make an accurate robot. Just some .run_until_stalled(...) can do wonders already.
• I'ld say yes, the EV3 should be powerfull enough, as I've managed to get full Forward AND Inverse Kinematics working on the EV3 only without the use of a desktop. And this for a 6-axis robot, so a 3-axis should be no trouble.

When saving a waypoint, the Forward Kinematics are used to read the current XYZ/Roll/Pitch/Yaw position. Inverse kinematics are constantly used to create a straigth path between the waypoints.

• I found that the best way to write out these programs, is first make some sketches with old A4 paper and a pen how all joints work.

And I don't recommend to start writing out the Forward Kinematics for a 6axis at all.. this is all the Sin/Cos math just to find XYZ..

But a 3-axis should be wayyy more easy.

Anyway, enjoy your future projects with your EV3!

woah that is pretty amazing ive heard of using inverse kinematics in games and some robots eg heres one by a scrap mechanic youtuber been thinking of programming a simple simulation in scratch for this lol Ev3 projects are always amazing especially those by you mr jos!

 

[jamesaguilar]

4 hours ago, Mr Jos said:

I'ld say yes, the EV3 should be powerfull enough, as I've managed to get full Forward AND Inverse Kinematics working on the EV3 only without the use of a desktop

Awesome! So, one wrinkle here is that I'm trying to do everything in pybricks-micropython. To my understanding there are no other real programming environments available on more recent bricks like spike prime. Unfortunately, it seems like pybricks cannot load c modules, so no numpy and no fast cpu intensive operations. The other wrinkle is that I'd really like to use an numerical/iterative solution even though it's plainly unnecessary for three axis. If only because I want to experience what a 7dof robot maker would in terms of software implementation. 

Since the project is just a one-off throwaway, I'll probably stick with the server based approach. But it's amazing to see what is possible with ev3! If I decide to stick with mostly ev3 rather than primarily using the spike prime set I have on order then I will have to investigate the c libraries to see what they can do. 

[Mr Jos]

13 minutes ago, jamesaguilar said:

Awesome! So, one wrinkle here is that I'm trying to do everything in pybricks-micropython. To my understanding there are no other real programming environments available on more recent bricks like spike prime. Unfortunately, it seems like pybricks cannot load c modules, so no numpy and no fast cpu intensive operations. The other wrinkle is that I'd really like to use an numerical/iterative solution even though it's plainly unnecessary for three axis. If only because I want to experience what a 7dof robot maker would in terms of software implementation. 

Since the project is just a one-off throwaway, I'll probably stick with the server based approach. But it's amazing to see what is possible with ev3! If I decide to stick with mostly ev3 rather than primarily using the spike prime set I have on order then I will have to investigate the c libraries to see what they can do. 

All my machines are programmed with PyBricks MicroPython on the EV3, I've never used C. I know about the limitations it has like numpy not working, but that's why I made my own program for Inverse Kinematics, as calculate the inverse matrix isn't possible as far as I found with MicroPython.

You can see all my projects here on Eurobricks, on YouTube or Rebrickable. But all are programmed with MicroPython, by someone who didn't know how to program with it 3years ago.

I think this next link should be a playlist with all I've done so far. My programs / screenshots are also around the web somewhere.

As you can see, the PyBricks is a very strong environment, and very user friendly in my opinion.

 

"The other wrinkle is that I'd really like to use an numerical/iterative solution"

• That's something my 6DoF robot does. No hard coded motor settings/angles. Just give it any point in space in millimeters in all directions, and it will move there, with a nice "smooth" movement, keeping the wrist flat if needed. No need to calculate yourself the motor speeds/angles, the program takes care of that every 50ms +-, for all 6 motors constantly. So what you want to do, should be possible directly on the EV3.

[jamesaguilar]

49 minutes ago, Mr Jos said:

but that's why I made my own program for Inverse Kinematics, as calculate the inverse matrix isn't possible as far as I found with MicroPython.

When you say, "made your own program," I am understanding you to mean you made a python module that does all this by hand? SQP requires being able to solve systems of equations as well. You did that? And it runs fast enough to do an update every 50ms!? Very impressive. I did not think the EV3 was powerful enough. I may have another look at it, although it is possible that I'm not interested enough in the math to actually do that part on the device when a library is at hand on my desktop to do it all for me. ;)

[Mr Jos]

57 minutes ago, jamesaguilar said:

When you say, "made your own program," I am understanding you to mean you made a python module that does all this by hand? SQP requires being able to solve systems of equations as well. You did that? And it runs fast enough to do an update every 50ms!? Very impressive. I did not think the EV3 was powerful enough. I may have another look at it, although it is possible that I'm not interested enough in the math to actually do that part on the device when a library is at hand on my desktop to do it all for me. ;)

I'm not so deep into math/robotics, don't know exactly what SQP means. But the program I wrote for my 6DoF checks the distance from its current position to the new coördinates (X Y Z Roll Pitch Yaw). It has a set maximum distance to be covered by each calculation. So if the movements is just move from Y = 100 to Y = -150, a distance of 250mm needs to be travelled. This is divided by the step size (for example 7mm), so 250/7 = 36 points will be calculated to get there. So step 1 will be using Y = 93 in the inverse kinematics, step 2 Y = 86, ... and so on If there also was a shift in Z direction, but only 72mm for example, it will also start moving 2mm each calculation in Z, and the 7mm in Y. Meanwhile it also controls the roll/pitch/yaw. If the starting Yaw angle was 0°, and you wanted it to be 90° at the end of this coördinate, it will start 'yawing' at 90°/36 = 2,5° per step. This all happens simultaneously. Calculations are done constantly to see howmany degrees each motor has to turn from it's current position for the last calculated step. The largest amount of degrees sets that motor to 100% of requested power. Other motors get a %power depending on their respective % of angle they need to turn. This way all motors finish their movement exactly at the same time.

At the same time a calculation is done to predict when the movement will be finished (if a large motor needs to turn 270°, and it runs at 800°/second, it will take 337ms). The next calculation needs to be finished before this movement is finished, and to prevent start-stop jerking, a new set of target angles/speeds needs to be send like 50ms before the end of the movement to each motor.

Anyway, as it's hard to follow my writing, here you can find my project on GitHub that I sell with my instructions for my 6DoF. I think you'll be able to follow what happens with all the remarks added everywhere.

https://github.com/Mr-Jos-Technic/Lego-EV3-6DoF/blob/main/6dof_left_accurate.py

Maybe it can be some help for your 3DoF. you just need the part that handles theta1+2+3. You don't neeed the wrist (theta4+5+6).

Edit: Oh yeah, this is already an old program from me, I've learned better ways to program now, but don't really like to clean up this "mess", as I think I'll just break it.

Edited August 31, 2023 by Mr Jos

[jamesaguilar]

I see where the miscommunication was! :) What you have here is the closed/analytic solution to inverse kinematics of your arm. That means that given a particular position, you directly calculate the desired positions of each actuator.

What I want to do is use an optimization algorithm to do the same thing. With SQP and other optimization algorithms, you don't explicitly calculate the inverse kinematics. Instead, you just provide:

• A function that computes your distance from the goal in goal-space given the inputs. (This is just absolute difference between the goal position & attitude and the forward kinematics.)
• The gradient function for the distance function. (I.e., the partial derivative of the distance function w.r.t. each input variable.)
• A list of constraints (i.e. "this motor can't rotate beyond position y"). These constraints can be linear or nonlinear.
• The gradient function for each constraint, again w.r.t. each of the input variables.

The algorithm tries to find the global minimum of the distance function, which is to say it tries to find the setting of the motors that minimizes the distance from your desired position to the goal. But it can also incorporate things like obstacle avoidance via the constraint functions.

You can look at scipy.optimize.minimize to see the function I intend to use.

Edited August 31, 2023 by jamesaguilar

[Mr Jos]

15 minutes ago, jamesaguilar said:

I see where the miscommunication was! :) What you have here is the closed/analytic solution to inverse kinematics of your arm. That means that given a particular position, you directly calculate the desired positions of each actuator.

What I want to do is use an optimization algorithm to do the same thing. With SQP and other optimization algorithms, you don't explicitly calculate the inverse kinematics. Instead, you just provide:

• A function that computes your distance from the goal in goal-space given the inputs. (This is just absolute difference between the goal position & attitude and the forward kinematics.)
• The gradient function for the distance function. (I.e., the partial derivative of the distance function w.r.t. each input variable.)
• A list of constraints (i.e. "this motor can't rotate beyond position y"). These constraints can be linear or nonlinear.
• The gradient function for each constraint, again w.r.t. each of the input variables.

The algorithm tries to find the global minimum of the distance function, which is to say it tries to find the setting of the motors that minimizes the distance from your desired position to the goal. But it can also incorporate things like obstacle avoidance via the constraint functions.

You can look at scipy.optimize.minimize to see the function I intend to use.

That sounds like a fun project to do! Seems my robot does most of those steps, but my constraints list is pretty small haha. I wanted to make it more realistic, but "woooosh" my head was blown to do it with micropython 2 years ago. Now I just limit motor 2+3+5 to where I know it will mechanically hit itself. Having to add the size/orientation of the wrist was my plan, but never found the time to do so. And indeed, I just go in a straigth line, if impossible because it goes through its own center it will not bend out with the arm. All things that would be fun to program, but to many projects waiting to be finished. Maybe I first should have done like you, start with a 3-axis arm and make it perfect. If my GBC warehouse will be (ever) finished, I'll take a 2nd look at my 6DoF program (maybe).

Anyway, enjoy your progress with the EV3!

Edited August 31, 2023 by Mr Jos

[ord]

On 8/31/2023 at 1:44 PM, jamesaguilar said:

It's so exciting! I have scarcely built anything made out of Legos in twenty years, and it's truly a rush to be back.

Enjoy! Many useful and interesting parts have been released in the past 20 years in the Technic theme.

20 hours ago, jamesaguilar said:

I'd really like to use an numerical/iterative solution even though it's plainly unnecessary for three axis. If only because I want to experience what a 7dof robot maker would in terms of software implementation.

That would be great if you could make this work - it could open the door for (inverse kinematics for) 7dof robots the likes of which I haven't seen before in Lego...

[jamesaguilar]

Right now I'm struggling through stuff that probably seems very basic to you more experienced lego builders out there. For example, sensing the limits of rotation on each of the arms. I do not have enough mindstorms buttons to make proper limit switches like you would in a real robot arm, so my strategy up to now has been to drive the motor at the lowest possible torque until it hits a physical limit that I've installed for each extreme of each axis.

The problem is that the lowest amount of torque seems to still be quite a lot of torque. I'm nervous of damaging my pieces! I'm using this code to sense the limits. Please make any suggestions for how to improve things:

def calibrate_throw(motor: Motor, speed: float, torque_fraction: int = 40):
    motor.stop()
    before_limits = motor.motor.control.limits()
    motor.motor.control.limits(before_limits[0], before_limits[1], torque_fraction)

    target_ms_per_degree = abs(1000.0 / speed)
    stalled_ms_per_degree = target_ms_per_degree * 2
    quit_ms_per_degree = max(100, target_ms_per_degree * 3)
    avg_ms_per_degree = EWMA(target_ms_per_degree, 0.2)

    motor.run(speed)
    wait(100)
    angle_before = motor.angle()

    watch = StopWatch()
    watch.resume()
    while True:
        wait(50)
        angle_after = motor.angle()
        if angle_after != angle_before:
            diff = 1.0 * abs(angle_after - angle_before)
            ms_per_angle = watch.time() / diff
            for x in range(int(diff)):
                avg_ms_per_degree.record(ms_per_angle)
            watch.reset()

        print(watch.time())
        if watch.time() > quit_ms_per_degree:
            print('had to quit took too long to get degree: ', watch.time(), ' > ', quit_ms_per_degree)
            break
        if avg_ms_per_degree.avg() > stalled_ms_per_degree:
            print('had to quit average fell too low: ', avg_ms_per_degree.avg(), ' > ', stalled_ms_per_degree)
            break

        angle_before = angle_after

    motor.stop()
    motor.motor.control.limits(*before_limits)

 

[Mr Jos]

@jamesaguilar It seems like you tried to make your own stall detection? PyBricks already has that covered for you!

 

motor_name.run_until_stalled(100, then=Stop.BRAKE, duty_limit=40)

motor_name.reset_angle(0)

 

100 = the speed you want to use for running to a stall, 40 = max power allowed to keep running at the given speed, 0 = the angle you will save as the current position.

You can play a bit with these values, but it's the only 2 lines of code you will need! (Don't go to low on duty_limit value, it will give an error, as the motor does need power to rotate freely)

Edited September 1, 2023 by Mr Jos

[jamesaguilar]

Thanks @Mr Jos. I did try run_until_stalled with a duty_limit before, but it never seemed to detect that the motor was stalled! It's possible that the speed I was using wasn't as high though, or that I was using the wrong duty limit. I'll give it another crack tonight.

[ord]

@jamesaguilar there's definitely a sweet spot with the duty limit - for me 30 has worked (with speed 100). Putting it much higher has me worried too about damaging pieces. Of course, a well-built end-stop goes a long way :).

 

[jamesaguilar]

Okay, I managed to get something working that I'm reasonably happy with. It took a lot of tweaking, the ole python is pretty rusty. But, it behaves in a distinctly robot-arm like way at this point:

https://photos.app.goo.gl/9gYNdfNemzQ8pQnd7

What is happening here:

• Server program runs on EV3. It uses three bluetooth mailboxes: one to broadcast the (logical) range of its actuators, one to broadcast the actuators' current settings, and one to receive commands to move to a particular set of target angles in a particular number of milliseconds.
• Client program running on PC:
  • Inverse kinematics computed using scipy
  • Control algorithm creates a virtual point (starting at the initial real point of the arm). This point can move at most 5 lego units per second.
  • Commands received via game controller update the virtual point.
  • We ask scipy to solve the motor positions that will get us closest to the virtual point, using the current motor position as an initial guess.
  • We always send the new motor positions to the robot.
  • If scipy finds us a solution that is within one unit of bang on, we also update the virtual point with the new virtual point on each update.

IK code is surprisingly easy (if you stand on the shoulders of giants):

_r1 = 2.0
_l2 = 20.0
_l3 = 15.0

Vec3 = npt.NDArray[np.number[Any]]

def get_radius(input: Vec3) -> float:
    return _r1 + _l2 * sin(input[1]) + _l3 * sin(input[1] + input[2])


def get_pos(input: Vec3) -> Vec3:
    """Finds the X,Y,Z vector given the settings of each motor. The settings of the motors are given in radians from their neutral positions."""
    ar = input[0]
    a2 = input[1]
    a3 = input[2]

    r = get_radius(input)

    x = r * cos(ar)
    y = _l2 * cos(a2) + _l3 * cos(a2 + a3)
    z = r * sin(ar)
    ret = np.array((x, y, z))
    return ret


def get_err(input:Vec3, target: Vec3) -> np.floating:
    return norm(target - get_pos(input))


def get_motor_settings(target: Vec3, initial_guess: npt.ArrayLike):
    # I tried to differentiate this and I got a headache. Finite estimation methods ftw.
    initial_guess = initial_guess if initial_guess is not None else (0, 0, 0)
    return minimize(get_err, initial_guess, args=(target,), method="slsqp", jac="3-point", bounds=((radians(-90), radians(90)), (radians(17), radians(90)), (radians(-6), radians(161))))

Yep, that's really it.

The whole repository can be found here. There are a bunch of other things you could do. For example, recording points and moving to them, or making the calibration a little more effective. Or if you wanted to this could serve as a basis for a more complete robot arm.

One of the biggest things I learned from this project is that One Does Not Simply Build Technic. I thought, "how hard could it be?" And boy was I wrong. I had a lot of trouble accomplishing the shape of arm I desired compared to my expectations. I do not have any intuition for how to create joints properly or how to build up rigid shapes. If you take a look at the model, you'll notice that the bottom half is actually one of the starter projects for the EV3 set, not a design of my own. The arm is still mine, but I could not figure out how to get the base set up properly without some guidance.

I think for my next few projects, I'm going to work through all the Mindstorms starter projects, and maybe buy a few used Technic sets to start getting a sense of the building techniques and also gathering a piece repository. After I've done that, I'll start thinking about what I want to create next.

[Mr Jos]

Good to see you had fun doing this project!

I do see you made a little 'mistake'

    # Runs while arm1 is pointing up, so it should be safe to test our whole limit.
    lower_stall = arm2.run_until_stalled(-6 * arm2_min_speed, duty_limit=30)

    # We want the lower_stall angle to be -6 after the reset. This means that
    # we need to reset the current angle to be -6 - lower_stall + current_angle
    # For example, if we stalled at -27 and we are now at -36, then the reset angle
    # should be -36 - (-27) - 6 = -15
    arm2.reset_angle(arm2.angle() - lower_stall - 6)

You can simply write

   arm2.run_until_stalled(-6 * arm2_min_speed, duty_limit=30)

   arm2.reset_angle(- 6)

The reset_angle function just writes the given argument as the new current angle, so no need to know howmuch you moved or what the current angle value is. Just stall with the run_until_stalled function, and then reset with the value you want it to be.

Other parts of your program are more advanced than I know, as I teached myself by reading online. I don't know anything about Class, self, ... . And I see you used better if statements, I always used "If ... == True:" but I see "If ... :" should do the same, and it's more simple.

 

Enjoy your next projects, and hope to see more of them here!

[jamesaguilar]

15 minutes ago, Mr Jos said:

You can simply write

   arm2.run_until_stalled(-6 * arm2_min_speed, duty_limit=30)

   arm2.reset_angle(- 6)

Thank you for looking over it!

This code actually does not do the same thing, because the result of motor.run_until_stalled() is not the same as motor.angle() after motor.run_until_stalled(). `run_until_stalled` returns the angle at which the stall is estimated to have begun, whereas angle returns the current angle of the motor. Given the torque of the motor w.r.t. to the rigidity of the structure, this means that the difference between the two angles could be as much as five degrees, with angle() being an overshot measurement. So you need to adjust for that when you reset the angles.

17 minutes ago, Mr Jos said:

"If ... == True:" but I see "If ... :" should do the same, and it's more simple.

Ah yeah. I am a programmer at my day job so I'm very used to the coding part. It's the lego building part that I'm inexperienced in!

[Mr Jos]

3 hours ago, jamesaguilar said:

Thank you for looking over it!

This code actually does not do the same thing, because the result of motor.run_until_stalled() is not the same as motor.angle() after motor.run_until_stalled(). `run_until_stalled` returns the angle at which the stall is estimated to have begun, whereas angle returns the current angle of the motor. Given the torque of the motor w.r.t. to the rigidity of the structure, this means that the difference between the two angles could be as much as five degrees, with angle() being an overshot measurement. So you need to adjust for that when you reset the angles.

Ah yeah. I am a programmer at my day job so I'm very used to the coding part. It's the lego building part that I'm inexperienced in!

I don't use the run_until_stalled as a function to return the angle value. Just to position the motor (at the mechanical limit)

You have a few options with the run_until_stalled, either use a then=Stop.HOLD in the homing, this way it will keep the motor at the stalled angle (I mostly use this). Then just reset the angle to whatever it should be at this stalled and locked angle, and move the motor after the reset to release the stress from the motor. Other option is no holding, and adding a short pause "wait(300)". Then reset the angle, this will 'relax' the motor to the point where the stall started approx.

I use both of these options for all of my machines and have very accurate homing with it, every time again.

[jamesaguilar]

17 minutes ago, Mr Jos said:

I use both of these options for all of my machines and have very accurate homing with it, every time again.

That's fair. I don't aim to get you to change your practices. But, for what it's worth, the approach I'm using is the one that is recommended by the pybricks authors for maximum accuracy.