!pip install pypoly2tri idealab_tools foldable_robotics pynamics

%matplotlib inline

import pynamics
from pynamics.frame import Frame
from pynamics.variable_types import Differentiable,Constant
from pynamics.system import System
#from pynamics.body import Body
#from pynamics.dyadic import Dyadic
from pynamics.output import Output,PointsOutput
#from pynamics.particle import Particle
import pynamics.integration
import sympy
import numpy
import matplotlib.pyplot as plt
plt.ion()
from math import pi
#from pynamics.constraint import Constraint
import scipy.optimize
import math as m

system = System()
pynamics.set_system(__name__,system)

#Defining link lengths

#Base Structure
l0 = Constant(0.05715, 'l0',system) # 2.25" * 0.0254 m/in = 0.05715
l1 = Constant(0.22225, 'l1',system) # 8.75" = 0.22225m
l2 = Constant(0.1016, 'l2',system) # 4" = 0.1016m
l3 = Constant(0.12065, 'l3',system) # 4.75" = 0.12065m
ls = Constant(0.009525,'ls',system) # 0.375" = 0.009525m

#Legs
lL = Constant(0.0381, 'lL',system) # 1.5" = 0.0381m

"""
#Leg Sections
lL = Constant(0.01905, 'lL',system) # 0.75" = 0.01905m
"""

# Creating dynamic state variables. system argument denotes them as state variables for pynamics system from above

#Frame angle from N
qA,qA_d,qA_dd = Differentiable('qA',system) #Angle between N and A

#Frame position
x1,x1_d,x1_dd = Differentiable('x1',system) #Position of x1, y1, which are at midpoint of L1
y1,y1_d,y1_dd = Differentiable('y1',system)

#Leg angles
q1,q1_d,q1_dd = Differentiable('q1',system)
q2,q2_d,q2_dd = Differentiable('q2',system)

#Sarrus extensions
s1,s1_d,s1_dd = Differentiable('s1',system)
s2,s2_d,s2_dd = Differentiable('s2',system)

"""
#Frame position
x1,x1_d,x1_dd = Differentiable('x1',system) #Position of x1, y1, which are at midpoint of L1
y1,y1_d,y1_dd = Differentiable('y1',system)

#Leg angles
q1,q1_d,q1_dd = Differentiable('q1',system)
q2,q2_d,q2_dd = Differentiable('q2',system)
q3,q3_d,q3_dd = Differentiable('q3',system)
q4,q4_d,q4_dd = Differentiable('q4',system)

#Sarrus extensions
s1,s1_d,s1_dd = Differentiable('s1',system)
s2,s2_d,s2_dd = Differentiable('s2',system)
"""

#Create an initial guess for their starting positions; not necessarily accurate, given the constraint that they are supposed to be connected with given, constant length
#Guesses are close but not quite exact for the desired configuration to show that it solves correctly

initialvalues = {}

#Frame constants
initialvalues[qA]=0*pi/180 
initialvalues[qA_d]=0*pi/180 

#Leg angle init Values
initialvalues[q1]=0*pi/180  
initialvalues[q1_d]=0*pi/180 
initialvalues[q2]=0*pi/180   
initialvalues[q2_d]=0*pi/180

#Sarrus linkages init extension
initialvalues[s1] = 0.0381 # 0.0127 multiplied by 2 to show some adjustment when solving
initialvalues[s1_d] = 0
initialvalues[s2] = 0.0381 # 1.5" = 0.0381m
initialvalues[s2_d] = 0

#Frame center
initialvalues[x1] = 0.13
initialvalues[x1_d] = 0 
initialvalues[y1] = .15
initialvalues[y1_d] = 0

"""
#Leg angle init Values
initialvalues[q1]=0*pi/180  
initialvalues[q1_d]=0*pi/180 
initialvalues[q2]=0*pi/180   
initialvalues[q2_d]=0*pi/180
initialvalues[q3]=0*pi/180 
initialvalues[q3_d]=0*pi/180
initialvalues[q4]=0*pi/180   
initialvalues[q4_d]=0*pi/180

#Sarrus linkages init extension
initialvalues[s1] = 0.0127*2 # multiplied by 2 to show some adjustment when solving
initialvalues[s1_d] = 0
initialvalues[s2] = 0.0127
initialvalues[s2_d] = 0

#Frame contact points
initialvalues[x1] = 0.13
initialvalues[x1_d] = 0 
initialvalues[y1] = .15
initialvalues[y1_d] = 0
"""

#Retrieve state variables in the order they are stored in the system
statevariables = system.get_state_variables()

# Initializing reference frames
N = Frame('N')

A = Frame('A')

L1 = Frame('L1')
L2 = Frame('L2')

"""
L1 = Frame('L1')
L2 = Frame('L2')
L3 = Frame('L3')
L4 = Frame('L4')
"""

#Set N frame as the newtonian
system.set_newtonian(N)

#Declare axis that reference frames are rotated about

#Frame axis A
A.rotate_fixed_axis_directed(N,[0,0,1],qA,system)

#Leg
#left side of leg from sarrus 1
L1.rotate_fixed_axis_directed(A,[0,0,1],q1,system)
#right side of leg from sarrus 2
L2.rotate_fixed_axis_directed(A,[0,0,1],q2,system)


"""
#Leg
#left side of leg from sarrus 1
L1.rotate_fixed_axis_directed(A,[0,0,1],q1,system)
L2.rotate_fixed_axis_directed(L1,[0,0,1],q2,system)
#right side of leg from sarrus 2
L4.rotate_fixed_axis_directed(A,[0,0,1],q4,system)
L3.rotate_fixed_axis_directed(L4,[0,0,1],q3,system)
"""

#Define vectors/points of the mechanism based on frames and previous vectors

#Center of outer frame
pAcm = x1*N.x + y1*N.y

#Sarrus extension frames
ps1 = pAcm - ls*A.x
ps2 = pAcm + ls*A.x

#Outer frame
pB = ps1  -l2*A.x
pC = ps2  + l2*A.x
pA = pB - l0*A.y
pD = pC - l0*A.y

#Legs
pL1 = ps1 - s1*A.y
pL2 = ps2 - s2*A.y

#Leg tips/foot
pLtip1 = pL1 - lL*L1.y
pLtip2 = pL2 - lL*L2.y

#Arbitrary pout that represents the end effector, here it is  pFE. The system moves in one direction
pout = pLtip1

"""
#Center of outer frame
pAcm = x1*N.x + y1*N.y

#Sarrus extension frames
ps1 = pAcm - ls*A.x
ps2 = pAcm + ls*A.x

#Outer frame
pB = ps1  -l2*A.x
pC = ps2  + l2*A.x
pA = pB - l0*A.y
pD = pC - l0*A.y

#Legs
pL1 = ps1 - s1*A.y
pL2 = pL1 - lL*L1.y
pL4 = ps2 - s2*A.y
pL3 = pL4 - lL*L4.y

#Leg tips/foot
pLtip1 = pL2 - lL*L2.y
pLtip2 = pL3 - lL*L3.y
"""

#Define points list for plotting
points = [pA,pB,ps1,pL1,pLtip1,pLtip2,pL2,ps2,pC,pD]

"""
points = [pA,pB,ps1,pL1,pL2,pLtip1,pLtip2,pL3,pL4,ps2,pC,pD]
"""

#Create list of initial variables
statevariables = system.get_state_variables()
ini0 = [initialvalues[item] for item in statevariables]

#Define constraint vectors that will be used to limit the motion of the system as desired

eq_vector = pLtip1 - pLtip2

#Create list eq and append errors into it for later optimization
eq = []

eq.append((eq_vector).dot(A.x))
eq.append((eq_vector).dot(A.y))

eq_d=[(system.derivative(item)) for item in eq]

#Define independent and dependent variables; # of items in qd should equal # of constraints above

qi = [s1,s2]
qd = [q1,q2]

"""
#qd = [q1,q2,q3,q4,qA,x1,y1]
"""

constants = system.constant_values.copy() # Recalls link lengths declared near beginning
defined = dict([(item,initialvalues[item]) for item in qi])
constants.update(defined)
constants # Stores the names of constants/known values along with their values

#substitute constants in equation
eq = [item.subs(constants) for item in eq] # substitutes the values from constants into the equation eq

#convert to numpy array and sum the error
error = (numpy.array(eq)**2).sum()

#Convert to a function that scipy can use. Sympy has a “labmdify” function that evaluates an expression, but scipy needs a slightly different format.
f = sympy.lambdify(qd,error)

def function(args):
    return f(*args)

guess = [initialvalues[item] for item in qd]

result = scipy.optimize.minimize(function,guess)
#if result.fun>1e-3:
    #raise(Exception("out of tolerance"))

result.fun # if 0, then solves fine

ini = []
for item in system.get_state_variables():
    if item in qd:
        ini.append(result.x[qd.index(item)])
    else:
        ini.append(initialvalues[item])

points = PointsOutput(points, constant_values=system.constant_values)
points.calc(numpy.array([ini0,ini]))
points.plot_time()

eq_d = sympy.Matrix(eq_d)

qi = sympy.Matrix([s1_d,s2_d])
qd = sympy.Matrix([q1_d,q2_d])

"""
qi = sympy.Matrix([s1_d,s2_d])
qd = sympy.Matrix([q1_d,q2_d,q3_d,q4_d,qA_d,x1_d,y1_d])
"""

AA = eq_d.jacobian(qi)
BB = eq_d.jacobian(qd)

BB.simplify() #simplify expression so we can actually run jacobian smoothly
BB

J = -BB.inv()*AA
J  #note, somewhat long; simplify helps to comprehend

J.simplify()
J

qd2 = J*qi
qd2

subs = dict([(ii,jj) for ii,jj in zip(qd,qd2)])
subs

pout #pBC end effector

vout = pout.time_derivative()
vout

vout = vout.subs(subs)
vout

F_ee

F_ee = numpy.array([0, 0.5]).T # Arbitrary force vector on end effector
F_in = J.T.dot(F_ee)
F_in

vout

"""
angA = 135*pi/180 #radians
angF = 45*pi/180 #radians
length0 = 0.0254 #meters
v_in = 0.02 # meters/sec
r = 0.0254
w = -v_in/r
v_out = length0 * w * m.cos(angA)/m.cos(angF)
v_out
"""

power = F_in * v_out
power