Step 3 : Simultaneous optimization
Optimization problem
The simultaneous optimization problem is to solve the CTR design and motion planning simultaneously. In this problem, multiple robot configurations will be optimized in order to find a robot design that can follow a set of waypoints while satisfy the anatomical constraints.
The CTR simultaneous 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
'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.tubestraight_comp import TubestraightComp
from ctr_framework.tiporientation_comp import TiporientationComp
from ctr_framework.baseplanar_comp import BaseplanarComp
from ctr_framework.maxcurvature_comp import MaxcurvatureComp
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_simul import trianglemesh
class CtrsimulGroup(om.Group):
'''
CtrsimulGroup is a OpenMDAO Group object that compassed 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. CtrsimulGroup simultaneously optimize the number of k inverse kinematics problem and find
the optimal design.
'''
def initialize(self):
self.options.declare('num_nodes', default=100, types=int)
self.options.declare('k', default=1, 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('target')
self.options.declare('rotx_init')
self.options.declare('roty_init')
self.options.declare('rotz_init')
self.options.declare('base')
self.options.declare('equ_paras')
self.options.declare('pt_full')
self.options.declare('viapts_nbr')
self.options.declare('zeta')
self.options.declare('rho')
self.options.declare('lag')
self.options.declare('meshfile')
def setup(self):
num_nodes = self.options['num_nodes']
k = self.options['k']
k_ = self.options['k_']
i = self.options['i']
a = self.options['a']
pt = self.options['pt']
equ_paras = self.options['equ_paras']
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']
pt_full = self.options['pt_full']
viapts_nbr = self.options['viapts_nbr']
base = self.options['base']
zeta = self.options['zeta']
rho = self.options['rho']
lag = self.options['lag']
meshfile = self.options['meshfile']
mesh = trianglemesh(num_nodes,k,meshfile)
p_ = mesh.p
normals = mesh.normals
tube_length_init_ = np.array([200, 120,65]).reshape((1,3)) + 100
beta_init_ = np.zeros((1,3))
beta_init_[:,0] = -280
beta_init_[:,1] = -205
beta_init_[:,2] = -155
dl0 = tube_length_init_ + beta_init_
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
init_guess = scipy.io.loadmat('simul.mat')
comp1 = IndepVarComp()
comp1.add_output('mesh_x', val = p_[:,0])
comp1.add_output('mesh_y', val = p_[:,1])
comp1.add_output('mesh_z', val = p_[:,2])
comp1.add_output('pt',val=pt)
self.add_subsystem('mesh_comp', comp1, promotes=['*'])
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'])
comp.add_output('beta', shape=(k,3),val=init_guess['beta'])
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' ,val=init_guess['loc'])
self.add_subsystem('input_comp', comp, promotes=['*'])
# add subsystem
'ctr'
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')
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=k)
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=k)
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)
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')
'''Constraints'''
bccomp = BcComp(num_nodes=num_nodes,k=k)
diametercomp = DiameterComp()
tubeclearancecomp = TubeclearanceComp()
tubestraightcomp = TubestraightComp()
desiredpointscomp = DesiredpointsComp(num_nodes=num_nodes,k=k)
baseplanarcomp = BaseplanarComp(num_nodes=num_nodes,k=k,equ_paras=equ_paras)
deployedlenghtcomp = DeployedlengthComp(k=k)
betacomp = BetaComp(k=k)
self.add_subsystem('BetaComp', betacomp, promotes=['*'])
self.add_subsystem('Baseplanarcomp', baseplanarcomp, promotes=['*'])
self.add_subsystem('BcComp', bccomp, promotes=['*'])
self.add_subsystem('Desiredpointscomp', desiredpointscomp, 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=['*'])
locnorm = LocnormComp(k=k_,num_nodes=num_nodes)
self.add_subsystem('LocnormComp', locnorm, promotes=['*'])
rotnorm = RotnormComp(k=k_)
self.add_subsystem('rotnormComp', rotnorm, promotes=['*'])
# strain
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 = 2)
straincomp = StrainComp(num_nodes = num_nodes,k=k,num_t= 2)
strainvirtualcomp = StrainvirtualComp(num_nodes = num_nodes,k=k,num_t= 2)
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.,
)
ksconstraintscomp1 = KSConstraintsComp(
in_name='strain_max',
out_name='strain_max1',
shape=(k,num_t,tube_nbr),
axis=0,
rho=100.,
)
ksconstraintsmincomp1 = KSConstraintsMinComp(
in_name='strain_min',
out_name='strain_min1',
shape=(k,num_t,tube_nbr),
axis=0,
rho=100.,
)
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_subsystem('KsconstraintsComp1', ksconstraintscomp1, promotes=['*'])
self.add_subsystem('KsconstraintsminComp1', ksconstraintsmincomp1, promotes=['*'])
# self.add_constraint('torsionconstraint', equals=0.)
# self.add_constraint('tiporientation', equals=0)
self.add_constraint('locnorm', upper=2)
self.add_constraint('deployedlength12constraint', lower=10)
self.add_constraint('deployedlength23constraint', lower=10)
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_max1',upper=0.08)
# self.add_constraint('strain_min1',lower=-0.08)
'''objective function'''
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=['*'])
# objectives
objscomp = ObjsComp(k=k,num_nodes=num_nodes,
zeta=zeta[-1],
rho=rho[-1],
eps_r=init_guess['eps_r'],
eps_p=init_guess['eps_p'],
eps_e = init_guess['eps_e'],
lag=lag,
norm1 = norm1,
norm2 = norm2,
norm3 = norm3,
norm4 = norm4,
norm5 = norm5,
)
self.add_subsystem('ObjsComp', objscomp, promotes=['*'])
self.add_objective('objs')
After the group is built, we now can solve the simultaneous optimization problem by running the optimizer and function below:
import numpy as np
import scipy
import os
from ctr_framework.design_method.sim_opt import sim_opt
# Initialize the number of number of links and waypoints
num_nodes = 50
k = 10
# robot initial pose
base = np.array([-10,35,20]).reshape((3,1))
rot = np.array([3.14,0,0]).reshape((3,1))
# mesh .PLY file
meshfile = 'trachea.PLY'
# run simultaneous optimization
sim_opt(num_nodes,k,base,rot,meshfile)