first commit by Henry

.vscode/settings.json

@@ -0,0 +1,3 @@
+{
+     "python.pythonPath": "/usr/local/bin/python3"
+}

Openmdao/_main_five_bar_opt.py

@@ -0,0 +1,73 @@
+import numpy as np
+import sympy as sp 
+'''
+setting up initial values to feed-------------------------
+'''
+from _sub_func import SineTrajectory, Ik_5_link
+
+x = SineTrajectory()[0]
+y = SineTrajectory()[1]
+
+thetaL = np.zeros(len(x),dtype=float)
+thetaR = np.zeros(len(x),dtype=float)
+
+for i in range(len(x)):
+    theta = Ik_5_link(x[i], y[i])
+    thetaL[i] = theta.x[0]
+    thetaR[i] = theta.x[1]
+
+'''
+model building---------------------------------------------
+'''
+import openmdao.api as om
+from openmdao.api import IndepVarComp
+from _sub_comp_fk             import FkComp
+from _sub_obj_bar_length      import BarLengthObj
+
+prob = om.Problem() 
+
+# during one iteration, l1, l2 would be scalar, while thetaL, R would be vector of number of point 
+comp = IndepVarComp()
+comp.add_output('l1', val=0.09) 
+comp.add_output('l2', val=0.16) 
+comp.add_output('thetaL', val=np.array(thetaL))
+comp.add_output('thetaR', val=thetaR)
+prob.model.add_subsystem('comp_i', comp)
+
+comp = FkComp()
+prob.model.add_subsystem('comp_fk', comp)
+
+comp = BarLengthObj()
+prob.model.add_subsystem('obj_bar', comp)
+
+prob.model.connect('comp_i.l1',['comp_fk.l1','obj_bar.l1'])
+prob.model.connect('comp_i.l2',['comp_fk.l2','obj_bar.l2'])
+prob.model.connect('comp_i.thetaL','comp_fk.thetaL')
+prob.model.connect('comp_i.thetaR','comp_fk.thetaR')
+
+'''
+setting up model----------------------------------------------
+'''
+from openmdao.api import Problem, ScipyOptimizeDriver
+
+prob.driver = om.ScipyOptimizeDriver() 
+prob.driver.options['optimizer'] = 'SLSQP'
+
+prob.model.add_design_var('comp_i.l1',lower=0)
+prob.model.add_design_var('comp_i.l2',lower=0)
+prob.model.add_design_var('comp_i.thetaL', lower=-0.2*np.pi, upper=0.2*np.pi)
+prob.model.add_design_var('comp_i.thetaR', lower=0.7*np.pi, upper=1.2*np.pi)
+
+prob.model.add_objective('obj_bar.length')
+
+prob.model.add_constraint('comp_fk.x',equals=x)
+prob.model.add_constraint('comp_fk.y',equals=y)
+
+prob.setup()
+prob.run_model()
+prob.model.list_outputs()
+
+prob.run_driver()
+print('L1=   ',prob['comp_i.l1'])
+print('L2=   ',prob['comp_i.l2'])
+print('Total=',prob['obj_bar.length'])

Openmdao/_sub_func.py

@@ -0,0 +1,100 @@
+import numpy as np 
+import matplotlib.pyplot as plt
+from scipy.optimize import minimize
+from scipy.optimize import fsolve
+
+def SineTrajectory(DownAMP = 0.01, UpperAMP = 0.05, StanceHeight = 0.18, StepLength = 0.12):
+
+     CurrentPercent = np.array([a/100+0.01 for a in range(100)])
+     StancePercent = 0.6
+     SwingPercent = 1 - StancePercent
+
+     x = np.zeros(len(CurrentPercent), dtype=float)
+     y = np.zeros(len(CurrentPercent), dtype=float)
+
+     for i in range(len(CurrentPercent)):
+          if (CurrentPercent[i] <= StancePercent):
+               x[i] = -(StepLength / 2) + (CurrentPercent[i] / StancePercent) * StepLength
+               y[i] =  DownAMP * np.sin(np.pi * CurrentPercent[i] / StancePercent) + StanceHeight
+          else:
+               x[i] = (StepLength / 2) - ((CurrentPercent[i] - StancePercent) / SwingPercent) * StepLength
+               y[i] = -UpperAMP * np.sin(np.pi * (CurrentPercent[i] - StancePercent) / SwingPercent) + StanceHeight
+
+     return x, y
+
+
+
+def Ik_5_link(x, y, l1 = 0.09, l2 = 0.16, w = 0.07):
+
+    def leg_wide(var):
+        return np.linalg.norm([var[0], var[1] - np.pi])
+
+    def x_constraint_equation(var):
+        # should be equal to zero when the 
+        return l1**2 - l2**2 + (x - w/2)**2 + y**2 - 2*l1*(y*np.sin(var[0]) + (x - w/2)*np.cos(var[0]))
+
+    def y_constraint_equation(var):
+        return l1**2 - l2**2 + (x + w/2)**2 + y**2 - 2*l1*(y*np.sin(var[1]) + (x + w/2)*np.cos(var[1]))
+
+
+    res = minimize(leg_wide, (0.1, 9*np.pi/10), method="SLSQP", constraints= ({"type": "eq", "fun": x_constraint_equation}, 
+                                                                               {"type": "eq", "fun": y_constraint_equation}))
+
+
+    return (res)
+
+def FkGetPosition(thetaL, thetaR, l1 = 0.09, l2 = 0.16):
+
+     w       = 0.07
+     a       = np.sqrt(w ** 2 + l1 ** 2 - 2 * w * l1 * np.cos(thetaR))
+     alpha   = np.arcsin(l1 * np.sin(thetaR) / a)
+     L       = np.sqrt(l1 ** 2 + a ** 2 - 2 * l1 * a * np.cos(np.pi - alpha - thetaL))
+     beta    = np.arcsin(a * np.sin(np.pi - alpha - thetaL) / L)
+     thetaL2 = np.pi - beta
+
+     '''
+     Transmission Matrix
+     '''
+     def T(theta, x, y): 
+          return np.array([[np.cos(theta), -np.sin(theta), x], 
+                           [np.sin(theta), np.cos(theta),  y],
+                           [0,             0,              1]])
+
+     # Fk = T(thetaL, w/2, 0).dot( T(thetaL2, l1, 0) ).dot(np.array( [L/2, -np.sqrt(l2 ** 2 - (L/2) ** 2), 1]) )
+     # Fk = multi_dot([T(thetaL, w/2, 0), T(thetaL2, l1, 0), np.array( [L/2, -np.sqrt(l2 ** 2 - (L/2) ** 2), 1])])
+     Fk = T(thetaL, w/2, 0) @ T(thetaL2, l1, 0) @ np.array( [L/2, -np.sqrt(l2 ** 2 - (L/2) ** 2), 1])
+     Fk = Fk[:2]
+
+     x = Fk[0]
+     y = Fk[1]
+
+     return x, y
+
+
+if __name__ == '__main__':
+
+     x = SineTrajectory()[0]
+     y = SineTrajectory()[1]
+
+     thetaL = np.zeros(len(x),dtype=float)
+     thetaR = np.zeros(len(x),dtype=float)
+
+     for i in range(len(x)):
+          theta = Ik_5_link(x[i], y[i])
+          # print(theta.x[0], theta.x[1])
+          thetaL[i] = theta.x[0]
+          thetaR[i] = theta.x[1]
+
+     x_ = FkGetPosition(thetaL, thetaR)[0]
+     y_ = FkGetPosition(thetaL, thetaR)[1]
+
+     plt.plot(x_, y_, x, y, marker='*')
+     plt.show()
+
+     j = np.min(y_)
+     print(j)
+
+     p = FkGetPosition(0, np.pi)
+     q = FkGetPosition(0.25*np.pi, 0.75*np.pi)
+     r = FkGetPosition(0.5*np.pi, 0.5*np.pi)
+     print(p, q, r)

Openmdao/_sub_obj_bar_length.py

@@ -0,0 +1,41 @@
+import numpy as np
+from openmdao.api import ExplicitComponent
+
+class BarLengthObj(ExplicitComponent):
+
+     def setup(self):
+          self.add_input('l1')
+          self.add_input('l2')
+
+          self.add_output('length')
+
+          self.declare_partials('length', 'l1', val=1)
+          self.declare_partials('length', 'l2', val=1)
+
+     def compute(self, inputs, outputs):
+          l1 = inputs['l1']
+          l2 = inputs['l2']
+          outputs['length'] = l1 + l2
+
+if __name__ == '__main__':
+     from openmdao.api import Group, Problem, IndepVarComp
+     group = Group()
+
+     comp = IndepVarComp()
+     comp.add_output('l1', val=0.09) 
+     comp.add_output('l2', val=0.16) 
+     group.add_subsystem('comp1', comp)
+
+     comp = BarLengthObj()
+     group.add_subsystem('comp2', comp)
+
+     group.connect('comp1.l1', 'comp2.l1')
+     group.connect('comp1.l2', 'comp2.l2')
+
+     prob = Problem()
+     prob.model = group
+
+     prob.setup()
+     prob.run_model()
+     prob.model.list_outputs()
+     prob.check_partials(compact_print=True)

Openmdao/_sub_partials.py

@@ -0,0 +1,52 @@
+import numpy as np 
+import sympy as sp
+import pprint
+from sympy.utilities.lambdify import lambdify 
+
+thetaL = sp.symbols('thetaL')
+thetaR = sp.symbols('thetaR')
+l1 = sp.symbols('l1')
+l2 = sp.symbols('l2')
+w = 0.07
+
+a       = sp.sqrt(w ** 2 + l1 ** 2 - 2 * w * l1 * sp.cos(thetaR))
+alpha   = sp.asin(l1 * sp.sin(thetaR) / a)
+L       = sp.sqrt(l1 ** 2 + a ** 2 - 2 * l1 * a * sp.cos(sp.pi - alpha - thetaL))
+beta    = sp.asin(a * sp.sin(sp.pi - alpha - thetaL) / L)
+thetaL2 = sp.pi - beta
+
+def T(theta, x, y): 
+    return sp.Matrix([[sp.cos(theta), -sp.sin(theta), x], 
+                    [sp.sin(theta), sp.cos(theta),  y],
+                    [0,             0,              1]])
+
+Fk = T(thetaL, w/2, 0) @ T(thetaL2, l1, 0) @ sp.Matrix([L/2, -sp.sqrt(l2 ** 2 - (L/2) ** 2), 1])
+Fk = Fk[:2,:]
+
+'''
+px_pl1 px_pl2
+py_pl1 py_pl2
+'''
+px_pl = Fk.jacobian([l1,l2]).evalf()
+# np.array(px_pl.tolist()).astype(np.float64)
+# px_pl1 = px_pl[0,0]
+# px_pl2 = px_pl[0,1]
+# py_pl1 = px_pl[1,0]
+# py_pl2 = px_pl[1,1]
+
+print('shape of px_pl:',np.shape(px_pl))
+# pprint.pprint(px_pl[1,1])
+
+'''
+px_pthetaL px_pthetaR
+py_pthetaL py_pthetaR
+'''
+px_ptheta = Fk.jacobian([thetaL,thetaR]).evalf()
+# np.array(px_ptheta).astype(np.float64)
+# px_pthetaL = px_ptheta[0,0]
+# px_pthetaR = px_ptheta[0,1]
+# py_pthetaL = px_ptheta[1,0]
+# py_pthetaR = px_ptheta[1,1]
+
+print('shape of px_pθ:',np.shape(px_ptheta))
+# pprint.pprint(px_ptheta[1,1])