Day 4: (Thu Sep 5) Kinematics Etc.

Notes for 2019-09-05. See also the Fall 2019 Calendar.

Notes from Day 3

Agenda

1. Administrative

   • If needed, please fix your post to use the code syntax highlighting as per code formatting in posts

2. Assignments

3. In-class

   1. Demos for Exercise: Dynamic Gesture on a Simulated Double Pendulum

   2. Brief numpy tutorial.

   3. Discussion of two-link kinematics.

   4. A few videos.

   5. Discussion of common platform ideas.

Videos

• Claude Shannon

• ETH Zurich Blind Juggler

• Virk-I Wooden SCARA Robot Arm

• SCARA robot demo :https://www.youtube.com/watch?v=k_nHFSdk8Sg

• SCARA robots demo: https://www.youtube.com/watch?v=4RQiGz-ygsQ

Lecture code samples

NumPy

We may use some of the NumPy library for numerical computing, which isn’t part of core Python.

# Make the numpy library available under the name 'np'.
import numpy as np

# Create a 3-element zero vector (all floating point)
z = np.zeros((3))
print(z)

# Create a 3-element zero vector (integer)
z = np.zeros((3), dtype=np.int)
print(z)

# Create a 3x4 zero matrix (3 rows, 4 columns).
z = np.zeros((3,4))
print(z)

# Create an uninitialized 2x3 matrix (2 rows, 3 columns).
u = np.ndarray((2,3))
print(u)

# Create a 3x3 identity matrix.
I = np.array(((1.0,0,0),(0,1.0,0),(0,0,1.0)))
print(I)

# Create a 3x3 identity matrix.
I = np.eye(3)
print(I)

# Create a random 3-element vector.
R = np.random.rand(3)
print(R)

# Multiply by a scalar.
print(10 * R)

# Vector arithmetic.  The following operate element-by-element.

print(10 * R**2 + R)

print(np.sin(R))


# The same notation provides efficient access to large arrays.

big = np.random.rand(10000)

# Calculate the magnitude of the high-dimensional vector:

magnitude = np.sqrt(np.dot(big,big))

print("Long vector magnitude:", magnitude)

# The same is available as a primitive:
print("Long vector magnitude:", np.linalg.norm(big))

# For more, see https://docs.scipy.org/doc/numpy/reference/routines.linalg.html

Reference manual items:

https://docs.scipy.org/doc/numpy/reference/index.html

https://docs.scipy.org/doc/numpy/reference/routines.linalg.html

And the broader SciPy:

https://docs.scipy.org/doc/scipy/reference/

Not to mention more specialized libraries:

https://scikit-learn.org/stable/user_guide.html

Two-Link Forward Kinematics

    def forwardKinematics(self, q):
        """Compute the forward kinematics.  Returns the world-coordinate Cartesian position of the elbow
and endpoint for a given joint angle vector.

        :param q: two-element list or ndarray with [q1, q2] joint angles
        :return: tuple (elbow, end) of two-element ndarrays with [x,y] locations
        """

        elbow = self.origin + np.array((self.l1 * math.sin(q[0]), -self.l1 * math.cos(q[0])))
        end   = elbow + np.array((self.l2 * math.sin(q[0]+q[1]), -self.l2 * math.cos(q[0]+q[1])))
        return elbow, end

Two-Link Inverse Kinematics

    def endpointIK(self, target):
        """Compute two inverse kinematics solutions for a target end position.  The
        target is a Cartesian position vector (two-element ndarray) in world coordinates,
        and the result vectors are joint angles as ndarrays [q0, q1].
        If the target is out of reach, returns the closest pose.
        """

        # translate the target vector into body coordinates
        target = target - self.origin
        
        # find the position of the point in polar coordinates
        radiussq = np.dot(target, target)
        radius   = math.sqrt(radiussq)

        # theta is the angle of target point w.r.t. -Y axis, same origin as arm
        theta    = math.atan2(target[0], -target[1]) 

        # use the law of cosines to compute the elbow angle
        #   R**2 = l1**2 + l2**2 - 2*l1*l2*cos(pi - elbow)
        #   both elbow and -elbow are valid solutions
        acosarg = (radiussq - self.l1**2 - self.l2**2) / (-2 * self.l1 * self.l2)
        if acosarg < -1.0:  elbow_supplement = math.pi
        elif acosarg > 1.0: elbow_supplement = 0.0
        else:               elbow_supplement = math.acos(acosarg)

        # use the law of sines to find the angle at the bottom vertex of the triangle defined by the links
        #  radius / sin(elbow_supplement)  = l2 / sin(alpha)
        if radius > 0.0:
            alpha = math.asin(self.l2 * math.sin(elbow_supplement) / radius)
        else:
            alpha = 0.0
            
        #  compute the two solutions with opposite elbow sign
        soln1 = np.array((theta - alpha, math.pi - elbow_supplement))
        soln2 = np.array((theta + alpha, elbow_supplement - math.pi))

        return soln1, soln2