In [4]:

    

import sympy
import numpy
from sympy import pprint


    

In [5]:

    

G, J = sympy.symbols('G J')					# Vector functions
q1, q2, x, y = sympy.symbols('q1 q2 x y')	# Degrees of freedom (two DOF), two dimensions for output space.
l1, l2 = sympy.symbols('l1 l2')				# System parameters (limb lengths)


    

In [6]:

    

x = l1*sympy.cos(q1) + l2*sympy.cos(q1+q2)
y = l1*sympy.sin(q1) + l2*sympy.sin(q1+q2)


    

In [7]:

    

G = sympy.Matrix([x,y])


    

In [8]:

    

J = G.jacobian([q1,q2])
J_inv = J.inv()
J_trans = J.transpose()
J_trans_inv = J.transpose().inv()


    

In [9]:

    

pprint(G)


    

    




⎡l₁⋅cos(q₁) + l₂⋅cos(q₁ + q₂)⎤
⎢                            ⎥
⎣l₁⋅sin(q₁) + l₂⋅sin(q₁ + q₂)⎦

In [10]:

    

pprint(J)


    

    




⎡-l₁⋅sin(q₁) - l₂⋅sin(q₁ + q₂)  -l₂⋅sin(q₁ + q₂)⎤
⎢                                               ⎥
⎣l₁⋅cos(q₁) + l₂⋅cos(q₁ + q₂)   l₂⋅cos(q₁ + q₂) ⎦

In [11]:

    

pprint(J_inv)


    

    




⎡              1                 (l₁⋅sin(q₁) + l₂⋅sin(q₁ + q₂))⋅(l₁⋅cos(q₁) + 
⎢───────────────────────────── + ─────────────────────────────────────────────
⎢-l₁⋅sin(q₁) - l₂⋅sin(q₁ + q₂)                                                
⎢                                               l₁⋅(-l₁⋅sin(q₁) - l₂⋅sin(q₁ + 
⎢                                                                             
⎢                      (l₁⋅sin(q₁) + l₂⋅sin(q₁ + q₂))⋅(l₁⋅cos(q₁) + l₂⋅cos(q₁ 
⎢                      ───────────────────────────────────────────────────────
⎣                              l₁⋅l₂⋅(-l₁⋅sin(q₁) - l₂⋅sin(q₁ + q₂))⋅sin(q₂)  

l₂⋅cos(q₁ + q₂))⋅sin(q₁ + q₂)  -(l₁⋅sin(q₁) + l₂⋅sin(q₁ + q₂))⋅sin(q₁ + q₂) ⎤
─────────────────────────────  ─────────────────────────────────────────────⎥
    2                            l₁⋅(-l₁⋅sin(q₁) - l₂⋅sin(q₁ + q₂))⋅sin(q₂) ⎥
q₂)) ⋅sin(q₂)                                                               ⎥
                                                                            ⎥
+ q₂))                               -(l₁⋅sin(q₁) + l₂⋅sin(q₁ + q₂))        ⎥
──────                               ────────────────────────────────       ⎥
                                              l₁⋅l₂⋅sin(q₂)                 ⎦

In [12]:

    

pprint(J_trans)


    

    




⎡-l₁⋅sin(q₁) - l₂⋅sin(q₁ + q₂)  l₁⋅cos(q₁) + l₂⋅cos(q₁ + q₂)⎤
⎢                                                           ⎥
⎣      -l₂⋅sin(q₁ + q₂)               l₂⋅cos(q₁ + q₂)       ⎦

In [13]:

    

pprint(J_trans_inv)
print "==================" #Finish matrix visualization


    

    




⎡              1                 (l₁⋅sin(q₁) + l₂⋅sin(q₁ + q₂))⋅(l₁⋅cos(q₁) + 
⎢───────────────────────────── + ─────────────────────────────────────────────
⎢-l₁⋅sin(q₁) - l₂⋅sin(q₁ + q₂)                                                
⎢                                               l₁⋅(-l₁⋅sin(q₁) - l₂⋅sin(q₁ + 
⎢                                                                             
⎢                              -(l₁⋅sin(q₁) + l₂⋅sin(q₁ + q₂))⋅sin(q₁ + q₂)   
⎢                              ─────────────────────────────────────────────  
⎣                                l₁⋅(-l₁⋅sin(q₁) - l₂⋅sin(q₁ + q₂))⋅sin(q₂)   

l₂⋅cos(q₁ + q₂))⋅sin(q₁ + q₂)  (l₁⋅sin(q₁) + l₂⋅sin(q₁ + q₂))⋅(l₁⋅cos(q₁) + l₂
─────────────────────────────  ───────────────────────────────────────────────
    2                                  l₁⋅l₂⋅(-l₁⋅sin(q₁) - l₂⋅sin(q₁ + q₂))⋅s
q₂)) ⋅sin(q₂)                                                                 
                                                                              
                                             -(l₁⋅sin(q₁) + l₂⋅sin(q₁ + q₂))  
                                             ──────────────────────────────── 
                                                      l₁⋅l₂⋅sin(q₂)           

⋅cos(q₁ + q₂))⎤
──────────────⎥
in(q₂)        ⎥
              ⎥
              ⎥
              ⎥
              ⎥
              ⎦
==================

In [13]:

    




    

In [14]:

    

# Define substitutions for Numerical Example
subs = {l1:1, #limb lengths
        l2:1,
        q1:0, #joint angles
        q2:numpy.pi/2.0}

print("Evaluate the functions for these parameter values \n")

# % Numerical examples
print('Numerical examples')

print('G \n')
valG = G.evalf(subs=subs)
pprint(valG)

print('J \n')
numerical_J = J.evalf(subs=subs)
pprint(numerical_J)

print('J_trans \n')
numerical_J_trans = J_trans.evalf(subs=subs)
pprint(numerical_J_trans)

print('J_inv \n')
numerical_J_inv = J_inv.evalf(subs=subs)
pprint(numerical_J_inv)

print('J_trans_inv \n')
numerical_J_trans_inv = J_trans_inv.evalf(subs=subs)
pprint(numerical_J_trans_inv)

# Numerical Examples
print('Example of applying a positive angular velocity at q1 to find the resulting instantaneous endpoint velocity')
q1_dot = 1
q2_dot = 0
q_dot = sympy.Matrix([q1_dot, q2_dot]).evalf(subs=subs)
print('q_dot')
pprint(q_dot)
x_dot = numerical_J * sympy.Matrix([q1_dot, q2_dot]).evalf(subs=subs)
x_dot = x_dot.n()
print('x_dot')
pprint(x_dot)

print('Example of applying that same endpoint velocity to find the resulting instantaneous angular velocities')
q_dot = numerical_J_inv * x_dot
q_dot = q_dot.evalf(subs=subs).n()
pprint(q_dot)

print('Example of finding which torques produce a horizontal endpoint force vector in equilibrium')
tau = numerical_J_inv.transpose() * sympy.Matrix([1, 0])
pprint(tau.n())

print('Example of applying those joint torques to find the resulting endpoint force vector in equilibrium')
# f = subs(J_trans_inv*tau)
f = numerical_J_trans_inv * tau
pprint(f.evalf())


    

    




Evaluate the functions for these parameter values 

Numerical examples
G 

⎡1.0⎤
⎢   ⎥
⎣1.0⎦
J 

⎡-1.0          -1.0        ⎤
⎢                          ⎥
⎣1.0   6.12323399573677e-17⎦
J_trans 

⎡-1.0          1.0         ⎤
⎢                          ⎥
⎣-1.0  6.12323399573677e-17⎦
J_inv 

⎡6.12323399573677e-17  1.0 ⎤
⎢                          ⎥
⎣        -1.0          -1.0⎦
J_trans_inv 

⎡6.12323399573677e-17  -1.0⎤
⎢                          ⎥
⎣        1.0           -1.0⎦
Example of applying a positive angular velocity at q1 to find the resulting instantaneous endpoint velocity
q_dot
⎡1.0⎤
⎢   ⎥
⎣ 0 ⎦
x_dot
⎡-1.0⎤
⎢    ⎥
⎣1.0 ⎦
Example of applying that same endpoint velocity to find the resulting instantaneous angular velocities
⎡1.0⎤
⎢   ⎥
⎣ 0 ⎦
Example of finding which torques produce a horizontal endpoint force vector in equilibrium
⎡6.12323399573677e-17⎤
⎢                    ⎥
⎣        1.0         ⎦
Example of applying those joint torques to find the resulting endpoint force vector in equilibrium
⎡-1.0⎤
⎢    ⎥
⎣-1.0⎦

In [14]: