Z OLD homework 10 - james-bern/CS345 GitHub Wiki

Hello

This is a 2+ week homework. We will be making metal robotic arms.

Notes

Test early; test often.
servo.write(...) is insufficient for task; https://forum.arduino.cc/t/servo-write-vs-servo-writemicroseconds/371642
You may want to make your pen spring-loaded so it doesn't "drag" behind the robot arm. You can make flexures and springs out of 3D-printed plastic.

Spec

A - A+
Finish your fidget spinner (no rush, just have it done by ~April 23)
Demo a reasonable robot arm in class on Thursday, April 25.
- Your robot arm will be judged based on...
  - How well it can write its name.
  - How well it can walk.
  - How well you can teleoperate it (using, for example, the laptop arrow keys) to solve a maze.
    - Maze will be drawn by me the day of.
- Your robot arm can use up to
  - 3x of the provided (regular; NOT continuous rotation) servos
  - 3x of the provided bearings
  - 6x of the provided long standoffs
  - 6x of the provided short standoffs
  - 1x 300mm x 300mm x 3mm plate of rather sus 6061 aluminum
    - https://www.amazon.com/Aluminum-Thickness-Rectangle-Protective-Polished/dp/B08YFJ1JV4/ref=cm_cr_arp_d_product_top?ie=UTF8
    - NOTE: Do NOT rely on the thickness of your plate being exactly what it says on the label!
- You can use 3D-printing and/or waterjet cutting.
  - ✨ If designing a metal arm, please first make a plastic copy so you can be sure it all fits together.
- For the competition, you can either clamp your robot arm down with one of the clamps or I can hold it down for you.
- For paper, we will use the sketchpad with "JIM" on it, which I have left next to my example robots.
- You must draw using a standard black sharpie. (Like the ones that have been on your desk.)

Mechanical Hints

Jim will help you ream and tap!--Don't try this by yourself!

Electronics Hints

(Unless you already have a board from your desk pet that you can repurpose) I highly recommend grabbing a fresh protoboard and soldering on...
- Female header for attaching the metro mini
- Female header for attaching the battery pack pins
- Male header for attaching the servos
- Solid core copper wire to connect anything you need to connect
If you aren't sure what this all means please ask! :)

Software Hints

Here was my development trajectory (we will review it in class):

draw the robot as two lines using forward kinematics
- NOTE: you can use pygame (https://www.pygame.org/docs/ref/draw.html)
be able to change the motor angles myself
be able to specify a target position
make the robot go to that target position using inverse kinematics
- you will probably want to use atan2: https://en.wikipedia.org/wiki/Atan2
- when passing an argument to acos, remember the range (y-value) of cos, goes from -1 to 1
  - if you're getting a "math domain error," you may want to do something like this:
```
arg_to_cos = ...
arg_to_cos = min(max(arg_to_cos, -1.0), 1.0) # clamp to [-1, 1]
... = acos(arg_to_cos)
```
- here is a really nice derivation: https://www.youtube.com/watch?v=IKOGwoJ2HLk
be able to specify a target trajectory (just a list of target points)
- NOTE: just because your simulation can reach a point doesn't mean the real robot can! the real servos can only spin so far! (pick an simple trajectory to start--maybe a relatively small box or a circle)
solve IK over and over to get the corresponding motor angle sequence
convert it into units that make sense for the real robot
- NOTE: this is tricky and hard to test (without just trying it on the real robot)! be careful! ask questions!
- NOTE: be sure you're using servo.writeMicroseconds(...) instead of the less precise servo.write(...); servo.write(...) is hot garbage for our purposes
get the motor sequence into an arduino program (i just stored it as a big array)
- NOTE: the metro mini has pretty severe memory limits; you may not be able to store as long of a sequence of motor angles as you would like

Inverse Kinematics App Pseudocode

global variables:
- target # this persists across frames so we can "warm-start" the optimization each frame

while true:                     # # a single frame of our app:
    target <- mousePosition     # pick a target (here, the mouse position)
    theta  <- IK(target, theta) # solve IK to convergence (gradient is ~0) starting from the previous choice of theta
    p      <- FK(theta)         # solve FK to find new position of the robot
    draw(p)                     # draw the new state of the robot

Pygame Forward Kinematics App

from math import *

##################################################
# PYGAME #########################################
##################################################

import pygame

pygame.init()
clock = pygame.time.Clock()

def begin_frame():
    pygame.display.flip()
    clock.tick(60)
    for event in pygame.event.get(): 
        if event.type == pygame.QUIT:
            return False
    screen.fill("white")
    return True


##################################################
# GRAPHICS #######################################
##################################################

BLACK = (  0,   0,   0)
RED   = (255,   0,   0)
GREEN = (  0, 255,   0)
BLUE  = (  0,   0, 255)

screen = pygame.display.set_mode([512, 512])

def pygame_from_world(p_world):
    return [256 + p_world[0], 256 - p_world[1]]

def world_from_pygame(p_pygame):
    return [-256 + p_pygame[0], 256 - p_pygame[1]]

def draw_line(a_world, b_world, color = BLACK):
    pygame.draw.line(screen, color, pygame_from_world(a_world), pygame_from_world(b_world), 5)

def draw_point(p_world, color = BLACK):
    pygame.draw.circle(screen, color, pygame_from_world(p_world), 8)


##################################################
# APP ############################################
##################################################

L1 = 100.0
L2 = 100.0
while begin_frame():
    mouse = world_from_pygame(pygame.mouse.get_pos())
    theta1 = radians(mouse[0])
    theta2 = radians(mouse[1])
    p1 = [L1 * cos(theta1), L1 * sin(theta1)]
    p2 = [p1[0] + L2 * cos(theta1 + theta2), p1[1] + L2 * sin(theta1 + theta2)]
    draw_line([0, 0], p1, RED)
    draw_line(p1, p2, BLUE)
    draw_point(mouse, GREEN)

Pygame Inverse Kinematics App (optimization version; 3-link arm; middle link can change its length)

from math import *
import numpy as np
from scipy.optimize import minimize

##################################################
# PYGAME #########################################
##################################################

import pygame

pygame.init()
clock = pygame.time.Clock()

def begin_frame():
    pygame.display.flip()
    clock.tick(60)
    for event in pygame.event.get(): 
        if event.type == pygame.QUIT:
            return False
    screen.fill("white")
    return True


##################################################
# GRAPHICS #######################################
##################################################

BLACK = (  0,   0,   0)
RED   = (255,   0,   0)
GREEN = (  0, 255,   0)
BLUE  = (  0,   0, 255)
BLUE  = (  0,   0, 255)

screen = pygame.display.set_mode([512, 512])

def pygame_from_world(p_world):
    return [256 + p_world[0], 256 - p_world[1]]

def world_from_pygame(p_pygame):
    return [-256 + p_pygame[0], 256 - p_pygame[1]]

def draw_line(a_world, b_world, color = BLACK):
    pygame.draw.line(screen, color, pygame_from_world(a_world), pygame_from_world(b_world), 5)

def draw_point(p_world, color = BLACK):
    pygame.draw.circle(screen, color, pygame_from_world(p_world), 8)


##################################################
# APP ############################################
##################################################

L0 = 50.0
L1 = 50.0
L2 = 50.0
theta = [ 0.0, 0.0, 0.0, 0.0 ]

def FK(theta):
    p1 = [L0 * cos(theta[0]), L0 * sin(theta[0])]
    p2 = [p1[0] + (L1 + theta[3]) * cos(theta[0] + theta[1]), p1[1] + (L1 + theta[3]) * sin(theta[0] + theta[1])]
    p3 = [p2[0] + L2 * cos(theta[0] + theta[1] + theta[2]), p2[1] + L2 * sin(theta[0] + theta[1] + theta[2])]
    return p1, p2, p3

def objective(theta):
    target = world_from_pygame(pygame.mouse.get_pos())
    _, _, p3 = FK(theta)
    dx = p3[0] - target[0]
    dy = p3[1] - target[1]
    return dx * dx + dy * dy + 0.1 * theta[3] * theta[3]

while begin_frame():
    mouse = world_from_pygame(pygame.mouse.get_pos())

    result = minimize(objective, np.array(theta), method = 'L-BFGS-B')
    theta = list(result['x'])

    p0 = [0.0, 0.0 ]
    p1, p2, p3 = FK(theta)
    draw_line(p0, p1, RED)
    draw_line(p1, p2, BLACK)
    draw_line(p2, p3, RED)
    draw_point(p0, BLUE)
    draw_point(p1, BLUE)
    draw_point(p2, BLUE)
    draw_point(mouse, GREEN)