Step 2 : Sequential optimization
Optimization problem
The sequential optimization step intends to solve a sequence of initial robot configurations from the start point to the target point. These configurations serve as an initial guesses for the simultaneous optimization step. The CtrseqGroup is an OpenMDAO group that consists of three ODE solver that are used to solve CTR kinematics probelm.
The CTR sequential optimization group is as follows:
import numpy as np
import scipy.io
import openmdao.api as om
from openmdao.api import Problem, Group, ExecComp, IndepVarComp, ScipyOptimizeDriver, pyOptSparseDriver
# from openmdao.api import Problem, Group, ExecComp, IndepVarComp, ScipyOptimizeDriver
from ozone.api import ODEIntegrator
from ctr_framework.stiffness_comp import StiffnessComp
from ctr_framework.CtrFunction import CtrFunction
from ctr_framework.tensor_comp import TensorComp
from ctr_framework.rhs_comp import RHSComp
from ctr_framework.kinematics_comp import KinematicsComp
from ctr_framework.k_comp import KComp
from ctr_framework.sumk_comp import SumkComp
from ctr_framework.sumkm_comp import SumkmComp
from ctr_framework.invsumk_comp import InvsumkComp
from ctr_framework.tubeends_comp import TubeendsComp
from ctr_framework.initpsi_comp import InitialpsiComp
from ctr_framework.penalize_comp import PenalizeComp
from ctr_framework.interpolationkp_comp import InterpolationkpComp
from ctr_framework.straightends_comp import StraightendsComp
from ctr_framework.kappa_comp import KappaComp
from ctr_framework.kout_comp import KoutComp
from ctr_framework.interpolationkb_comp import InterpolationkbComp
from ctr_framework.interpolationkp_comp import InterpolationkpComp
'backbone comps'
from ctr_framework.backbonefunction import BackboneFunction
from ctr_framework.initR_comp import InitialRComp
from ctr_framework.u1_comp import U1Comp
from ctr_framework.u2_comp import U2Comp
from ctr_framework.u3_comp import U3Comp
from ctr_framework.u_comp import UComp
from ctr_framework.uhat_comp import UhatComp
from ctr_framework.bborientation import BborientationComp
from ctr_framework.backboneptsFunction import BackboneptsFunction
'Integrator'
from ctr_framework.finaltime_comp import FinaltimeComp
'base angle'
from ctr_framework.baseangle_comp import BaseangleComp
from ctr_framework.rotp_comp import RotpComp
from ctr_framework.baseplanar_comp import BaseplanarComp
'constraints'
from ctr_framework.diameter_comp import DiameterComp
from ctr_framework.tubeclearance_comp import TubeclearanceComp
from ctr_framework.bc_comp import BcComp
from ctr_framework.desiredpoints_comp import DesiredpointsComp
from ctr_framework.deployedlength_comp import DeployedlengthComp
from ctr_framework.beta_comp import BetaComp
from ctr_framework.pathpoints_comp import PathpointsComp
from ctr_framework.tubestraight_comp import TubestraightComp
from ctr_framework.tiporientation_comp import TiporientationComp
from ctr_framework.chi_comp import ChiComp
from ctr_framework.gamma_comp import GammaComp
from ctr_framework.kappaeq_comp import KappaeqComp
from ctr_framework.strain_comp import StrainComp
from ctr_framework.ksconstraints_comp import KSConstraintsComp
from ctr_framework.ksconstraints_min_comp import KSConstraintsMinComp
from ctr_framework.reducedimension_comp import ReducedimensionComp
from ctr_framework.strainvirtual_comp import StrainvirtualComp
'objective'
from ctr_framework.objs_comp import ObjsComp
from ctr_framework.equdply_comp import EqudplyComp
from ctr_framework.reachtargetpts_comp import ReachtargetptsComp
from ctr_framework.targetnorm_comp import TargetnormComp
from ctr_framework.jointvaluereg_comp import JointvalueregComp
from ctr_framework.locnorm_comp import LocnormComp
from ctr_framework.rotnorm_comp import RotnormComp
from ctr_framework.dp_comp import DpComp
from ctr_framework.crosssection_comp import CrosssectionComp
from ctr_framework.signedfun_comp import SignedfunComp
'mesh'
from ctr_framework.mesh import trianglemesh
class CtrseqGroup(om.Group):
'''
CtrseqGroup is a OpenMDAO Group object that all the necessary components for solving the
CTR design optimization problem. This group includes the CTR kinematics model, kinematics constraints,
task-specific constraints and objectives, which the user is able to develop their own component and add
into the group. CtrseqGroup aims to obtain a sequence of robot configurations as initial guesses for the next
step.
'''
def initialize(self):
self.options.declare('num_nodes', default=100, types=int)
self.options.declare('k', default=1, types=int)
self.options.declare('i')
self.options.declare('a', default=2, types=int)
self.options.declare('tube_nbr', default=3, types=int)
self.options.declare('pt')
self.options.declare('pt_test')
self.options.declare('target')
self.options.declare('zeta')
self.options.declare('rho')
self.options.declare('eps_e')
self.options.declare('eps_r')
self.options.declare('eps_p')
self.options.declare('lag')
self.options.declare('rotx_init')
self.options.declare('roty_init')
self.options.declare('rotz_init')
self.options.declare('base')
self.options.declare('count')
self.options.declare('equ_paras')
self.options.declare('center')
self.options.declare('pt_full')
self.options.declare('viapts_nbr')
self.options.declare('meshfile')
def setup(self):
num_nodes = self.options['num_nodes']
k = self.options['k']
i = self.options['i']
a = self.options['a']
equ_paras = self.options['equ_paras']
pt = self.options['pt']
pt_test = self.options['pt_test']
count = self.options['count']
zeta = self.options['zeta']
rho = self.options['rho']
eps_e = self.options['eps_e']
eps_r = self.options['eps_r']
eps_p = self.options['eps_p']
lag = self.options['lag']
target = self.options['target']
tube_nbr = self.options['tube_nbr']
rotx_init = self.options['rotx_init']
roty_init = self.options['roty_init']
rotz_init = self.options['rotz_init']
base = self.options['base']
center = self.options['center']
pt_full = self.options['pt_full']
viapts_nbr = self.options['viapts_nbr']
meshfile = self.options['meshfile']
# mesh processing
mesh = trianglemesh(num_nodes,k,pt,center,meshfile)
p_ = mesh.p
normals = mesh.normals
if i == 0:
init_guess = scipy.io.loadmat('initial.mat')
else:
init_guess = scipy.io.loadmat('seq_'+str(i-1)+'.mat')
comp = IndepVarComp(num_nodes=num_nodes,k=k)
comp.add_output('d1', val=init_guess['d1'])
comp.add_output('d2', val=init_guess['d2'])
comp.add_output('d3', val=init_guess['d3'])
comp.add_output('d4', val=init_guess['d4'])
comp.add_output('d5', val=init_guess['d5'])
comp.add_output('d6', val=init_guess['d6'])
comp.add_output('kappa', shape=(1,3), val=init_guess['kappa'])
comp.add_output('tube_section_length',shape=(1,3),val=init_guess['tube_section_length'])
comp.add_output('tube_section_straight',shape=(1,3),val=init_guess['tube_section_straight'])
comp.add_output('lota', shape=(k,3),val=init_guess['lota'])
tp = init_guess['beta']
tp[:,0] = tp[:,0]+10
comp.add_output('beta', shape=(k,3),val=tp)
comp.add_output('initial_condition_dpsi', shape=(k,3), val=init_guess['initial_condition_dpsi'])
comp.add_output('rotx',val=init_guess['rotx'])
comp.add_output('roty',val=init_guess['roty'])
comp.add_output('rotz',val=init_guess['rotz'])
comp.add_output('loc',shape=(3,1),val=init_guess['loc'])
self.add_subsystem('input_comp', comp, promotes=['*'])
# add subsystem
'tube twist'
stiffness_comp = StiffnessComp()
self.add_subsystem('stiffness_comp', stiffness_comp, promotes=['*'])
tube_ends_comp = TubeendsComp(num_nodes=num_nodes,k=k,a=a)
self.add_subsystem('tube_ends_comp', tube_ends_comp, promotes=['*'])
interpolationkb_comp = InterpolationkbComp(num_nodes=num_nodes,k=k)
self.add_subsystem('interpolationkb_comp', interpolationkb_comp, promotes=['*'])
tensor_comp = TensorComp(num_nodes=num_nodes,k=k)
self.add_subsystem('tensor_comp', tensor_comp, promotes=['*'])
sumkm_comp = SumkmComp(num_nodes=num_nodes,k=k)
self.add_subsystem('sumkm_comp', sumkm_comp, promotes=['*'])
invsumk_comp = InvsumkComp(num_nodes=num_nodes,k=k)
self.add_subsystem('invsumk_comp', invsumk_comp, promotes=['*'])
k_comp = KComp(num_nodes=num_nodes, k=k)
self.add_subsystem('k_comp', k_comp, promotes=['*'])
straightedns_comp = StraightendsComp(num_nodes=num_nodes, k=k,a=a)
self.add_subsystem('straightends_comp', straightedns_comp, promotes=['*'])
interpolationkp_comp = InterpolationkpComp(num_nodes=num_nodes,k=k)
self.add_subsystem('interpolationkp_comp', interpolationkp_comp, promotes=['*'])
kappa_comp = KappaComp(num_nodes=num_nodes, k=k)
self.add_subsystem('kappa_comp', kappa_comp, promotes=['*'])
kout_comp = KoutComp(num_nodes=num_nodes, k=k)
self.add_subsystem('kout_comp', kout_comp, promotes=['*'])
# initialpsi_comp = InitialpsiComp(num_nodes=num_nodes,k=k)
# self.add_subsystem('initialpsi_comp', initialpsi_comp, promotes=['*'])
finaltime_comp = FinaltimeComp()
self.add_subsystem('final_comp', finaltime_comp, promotes=['*'])
method_name = 'Lobatto2'
'ODE 1 : kinematics'
ode_function1 = CtrFunction(k=1)
formulation1 = 'time-marching'
initial_time = 0.
normalized_times = np.linspace(0., 1, num_nodes)
integrator1 = ODEIntegrator(
ode_function1, formulation1, method_name,
final_time=initial_time, normalized_times=normalized_times
)
self.add_subsystem('integrator_group1', integrator1)
self.connect('final_time', 'integrator_group1.initial_time')
self.connect('K_out', 'integrator_group1.dynamic_parameter:K_out')
self.connect('lota', 'integrator_group1.initial_condition:psi')
self.connect('initial_condition_dpsi', 'integrator_group1.initial_condition:dpsi_ds')
self.connect('integrator_group1.state:dpsi_ds','dpsi_ds')
self.connect('integrator_group1.state:psi','psi')
'backbone'
u1_comp = U1Comp(num_nodes=num_nodes,k=k)
self.add_subsystem('u1_comp', u1_comp, promotes=['*'])
u2_comp = U2Comp(num_nodes=num_nodes,k=k)
self.add_subsystem('u2_comp', u2_comp, promotes=['*'])
u3_comp = U3Comp(num_nodes=num_nodes,k=k)
self.add_subsystem('u3_comp', u3_comp, promotes=['*'])
u_comp = UComp(num_nodes=num_nodes,k=k)
self.add_subsystem('u_comp', u_comp, promotes=['*'])
uhat_comp = UhatComp(num_nodes=num_nodes,k=k)
self.add_subsystem('uhat_comp', uhat_comp, promotes=['*'])
initR_comp = InitialRComp(num_nodes=num_nodes,k=k)
self.add_subsystem('initR_comp', initR_comp, promotes=['*'])
'ODE 2: Orientation'
ode_function2 = BackboneFunction(k=1)
formulation2 = 'time-marching'
initial_time = 0.
normalized_times = np.linspace(0., 1, num_nodes)
integrator2 = ODEIntegrator(
ode_function2, formulation2, method_name,
initial_time=initial_time, normalized_times=normalized_times
)
self.add_subsystem('integrator_group2', integrator2)
self.connect('final_time', 'integrator_group2.final_time')
self.connect('uhat', 'integrator_group2.dynamic_parameter:uhat')
self.connect('initial_condition_R', 'integrator_group2.initial_condition:R')
'ODE 3: Position'
ode_function3 = BackboneptsFunction(k=1)
formulation3 = 'time-marching'
initial_time = 0.
normalized_times = np.linspace(0., 1, num_nodes)
initial_conditions = { 'p': np.zeros((k,3,1))
}
integrator3 = ODEIntegrator(
ode_function3, formulation3, method_name,
initial_time=initial_time, normalized_times=normalized_times,
initial_conditions=initial_conditions
)
self.add_subsystem('integrator_group3', integrator3)
self.connect('final_time', 'integrator_group3.final_time')
self.connect('integrator_group2.state:R','integrator_group3.dynamic_parameter:R')
self.connect('integrator_group3.state:p','p')
'Transformation'
baseanglecomp = BaseangleComp(k=k,num_nodes=num_nodes,rotx_init=rotx_init,roty_init=roty_init,rotz_init=rotz_init)
self.add_subsystem('BaseangleComp', baseanglecomp, promotes=['*'])
rotpcomp = RotpComp(k=k,num_nodes=num_nodes,base=base)
self.add_subsystem('RotpComp', rotpcomp, promotes=['*'])
"Deisgn variables"
self.add_design_var('d1',lower= 0.2 , upper=3.5)
self.add_design_var('d2',lower= 0.2, upper=3.5)
self.add_design_var('d3',lower= 0.2, upper=3.5)
self.add_design_var('d4',lower= 0.2, upper=3.5)
self.add_design_var('d5',lower= 0.2, upper=3.5)
self.add_design_var('d6',lower= 0.2, upper=3.5)
tube_length_init = 0
tube_straight_init = 0
self.add_design_var('tube_section_length',lower=10)
self.add_design_var('tube_section_straight',lower=10 )
self.add_design_var('lota')
temp = np.outer(np.ones(k) , -init_guess['tube_section_length']+ 2)
self.add_design_var('beta', lower = temp,upper=-1)
self.add_design_var('kappa', lower=0)
# self.add_design_var('initial_condition_dpsi')
self.add_design_var('rotx')
self.add_design_var('roty')
# self.add_design_var('rotz')
self.add_design_var('loc')
locnorm = LocnormComp(k=k,num_nodes=num_nodes)
self.add_subsystem('LocnormComp', locnorm, promotes=['*'])
rotnorm = RotnormComp(k=k)
self.add_subsystem('rotnormComp', rotnorm, promotes=['*'])
'''Constraints'''
bccomp = BcComp(num_nodes=num_nodes,k=k)
diametercomp = DiameterComp()
tubeclearancecomp = TubeclearanceComp()
tubestraightcomp = TubestraightComp()
baseplanarcomp = BaseplanarComp(num_nodes=num_nodes,k=k,equ_paras=equ_paras)
# tiporientationcomp = TiporientationComp(k=k,tar_vector=tar_vector)
deployedlenghtcomp = DeployedlengthComp(k=k)
betacomp = BetaComp(k=k)
self.add_subsystem('BetaComp', betacomp, promotes=['*'])
self.add_subsystem('BcComp', bccomp, promotes=['*'])
self.add_subsystem('Baseplanarcomp', baseplanarcomp, promotes=['*'])
self.add_subsystem('DeployedlengthComp', deployedlenghtcomp, promotes=['*'])
self.add_subsystem('TubestraightComp', tubestraightcomp, promotes=['*'])
self.add_subsystem('DiameterComp', diametercomp, promotes=['*'])
self.add_subsystem('TubeclearanceComp', tubeclearancecomp, promotes=['*'])
# self.add_subsystem('TiporientationComp', tiporientationcomp, promotes=['*'])
#strain
num_t = 2
ksconstraintscomp = KSConstraintsComp(
in_name='strain_virtual',
out_name='strain_max',
shape=(num_nodes,k,num_t,tube_nbr),
axis=0,
rho=100.,
)
ksconstraintsmincomp = KSConstraintsMinComp(
in_name='strain_virtual',
out_name='strain_min',
shape=(num_nodes,k,num_t,tube_nbr),
axis=0,
rho=100.,
)
kappaeqcomp = KappaeqComp(num_nodes=num_nodes,k=k)
gammacomp = GammaComp(num_nodes=num_nodes,k=k)
chicomp = ChiComp(num_nodes=num_nodes,k=k,num_t = num_t)
straincomp = StrainComp(num_nodes = num_nodes,k=k,num_t= num_t)
strainvirtualcomp = StrainvirtualComp(num_nodes = num_nodes,k=k,num_t= num_t)
self.add_subsystem('KappaeqComp', kappaeqcomp, promotes=['*'])
self.add_subsystem('GammaComp', gammacomp, promotes=['*'])
self.add_subsystem('ChiComp', chicomp, promotes=['*'])
self.add_subsystem('StrainComp', straincomp, promotes=['*'])
self.add_subsystem('StrainvirtualComp', strainvirtualcomp, promotes=['*'])
self.add_subsystem('KsconstraintsComp', ksconstraintscomp, promotes=['*'])
self.add_subsystem('KsconstraintsminComp', ksconstraintsmincomp, promotes=['*'])
# self.add_constraint('torsionconstraint', equals=0.)
# self.add_constraint('tiporientation', equals=0)
self.add_constraint('locnorm', upper=2)
# self.add_constraint('baseconstraints', lower=0)
self.add_constraint('deployedlength12constraint', lower=1)
self.add_constraint('deployedlength23constraint', lower=1)
self.add_constraint('beta12constraint', upper=-1)
self.add_constraint('beta23constraint', upper=-1)
d_c = np.zeros((1,3)) + 0.1
self.add_constraint('diameterconstraint',lower= d_c)
self.add_constraint('tubeclearanceconstraint',lower= 0.1,upper=0.16)
# self.add_constraint('strain_max',upper=0.08)
# self.add_constraint('strain_min',lower = -0.08)
'''objectives'''
desiredpointscomp = DesiredpointsComp(num_nodes=num_nodes,k=k)
self.add_subsystem('Desiredpointscomp', desiredpointscomp, promotes=['*'])
reachtargetptscomp = ReachtargetptsComp(k=k,targets = pt)
self.add_subsystem('reachtargetptsComp', reachtargetptscomp, promotes=['*'])
targetnormcomp = TargetnormComp(k=k)
self.add_subsystem('Targetnormcomp', targetnormcomp, promotes=['*'])
dpcomp = DpComp(k=k,num_nodes=num_nodes,p_=p_)
self.add_subsystem('DpComp', dpcomp, promotes=['*'])
crosssectioncomp = CrosssectionComp(k=k,num_nodes=num_nodes)
self.add_subsystem('CrosssectionComp', crosssectioncomp, promotes=['*'])
signedfuncomp = SignedfunComp(k=k,num_nodes=num_nodes,normals=normals)
self.add_subsystem('SignedfunComp', signedfuncomp, promotes=['*'])
equdply = EqudplyComp(k=k,num_nodes=num_nodes)
self.add_subsystem('EqudplyComp', equdply, promotes=['*'])
# objective function
dl0 = init_guess['tube_section_length'] + init_guess['beta']
norm1 = np.linalg.norm(pt_full[0,:]-pt_full[-1,:],ord=1.125)
norm2 = (dl0[:,0] - dl0[:,1])**2 + (dl0[:,1] - dl0[:,2])**2
norm3 = np.linalg.norm(pt_full[0,:]-pt_full[-1,:])/viapts_nbr
norm4 = 2
norm5 = 2*np.pi
objscomp = ObjsComp(k=k,num_nodes=num_nodes,
zeta=zeta,
rho=rho,
eps_r=eps_r,
eps_p=eps_p,
lag=lag,
norm1 = norm1,
norm2 = norm2,
norm3 = norm3,
norm4 = norm4,
norm5 = norm5,
eps_e = eps_e,)
self.add_subsystem('ObjsComp', objscomp, promotes=['*'])
self.add_objective('objs')
After the group is built, we now can solve the sequential optimization problem by initializing the tube design, intial pose and running the optimizer:
import numpy as np
import scipy
import os
from openmdao.api import pyOptSparseDriver
from openmdao.api import ScipyOptimizeDriver
try:
from openmdao.api import pyOptSparseDriver
except:
pyOptSparseDriver = None
from ctr_framework.ctrseq_group import CtrseqGroup
from lsdo_viz.api import Problem
from ctr_framework.mesh import trianglemesh
from ctr_framework.initpt import initialize_pt
from ctr_framework.collision_check import collision_check
from ctr_framework.log import log
import shutil
import time
from ctr_framework.equofplane import equofplane
from ctr_framework.findcircle import findCircle
from ctr_framework.design_method.seq_opt import seq_opt
#########################################
############## initialization ###########
#########################################
# number of waypoints
viapts_nbr=10
k = 1
# number of links
num_nodes = 50
# Extract the waypoints from optimized path
pt = initialize_pt(viapts_nbr)
pt_pri = initialize_pt(viapts_nbr * 2)
pt_full = initialize_pt(100)
# initial robot configuration
# Tube 1(inner tube) ID, OD
d1 = 0.65
d2 = 0.88
# Tube 2
d3 = 1.076
d4 = 1.296
# Tube 3(outer tube)
d5 = 1.470
d6 = 2.180
# Tube curvature (kappa)
kappa_init = np.array([0.0061, 0.0131,0.0021]).reshape((1,3))
# The length of tubes
tube_length_init = np.array([200, 120,65]).reshape((1,3)) + 100
# The length of straight section of tubes
tube_straight_init = np.array([150, 80,35]).reshape((1,3)) + 50
# joint variables
alpha_init = np.zeros((k,3))
alpha_init[:,0] = -np.pi/2
alpha_init[:,1] = np.pi/1.5
alpha_init[:,2] = -np.pi/3
beta_init = np.zeros((k,3))
beta_init[:,0] = -280
beta_init[:,1] = -205
beta_init[:,2] = -155
# initial torsion
init_dpsi = np.random.random((k,3)) *0.01
rotx_ = 1e-10
roty_ = 1e-10
rotz_ = 1e-10
loc = np.ones((3,1)) * 1e-5
mdict = {'alpha':alpha_init, 'beta':beta_init,'kappa':kappa_init,
'tube_section_straight':tube_straight_init,'tube_section_length':tube_length_init,
'd1':d1, 'd2':d2, 'd3':d3, 'd4':d4, 'd5':d5, 'd6':d6, 'initial_condition_dpsi':init_dpsi,
'rotx':rotx_,'roty':roty_ ,'rotz':rotz_ , 'loc':loc,
}
scipy.io.savemat('initial.mat',mdict)
# Base frame
base = np.array([-10,35,20]).reshape((3,1))
rot = np.array([3.14,0,0]).reshape((3,1))
p_plane = np.array([[-10,35,20],[-12,20,20],\
[-20,15,20]])
# mesh .PLY file
meshfile = 'trachea.PLY'
seq_opt(num_nodes,viapts_nbr,base,rot,meshfile)