diff --git a/.gitignore b/.gitignore
index 5d05f18f..e32b85d4 100644
--- a/.gitignore
+++ b/.gitignore
@@ -39,3 +39,5 @@ ExperimentDataCheck
ExperimentVideo
Configuration.h
drake_binary
+*_RESULTS.yaml
+*.pyc
diff --git a/Addition/PythonPlotter/PlotCentroidTrajectory.py b/Addition/PythonPlotter/PlotCentroidTrajectory.py
new file mode 100644
index 00000000..25780d14
--- /dev/null
+++ b/Addition/PythonPlotter/PlotCentroidTrajectory.py
@@ -0,0 +1,47 @@
+#!/usr/bin/python
+
+'''
+ Copyright [2017] Max Planck Society. All rights reserved.
+
+ This program is free software: you can redistribute it and/or modify
+ it under the terms of the GNU General Public License as published by
+ the Free Software Foundation, either version 3 of the License, or
+ (at your option) any later version.
+
+ This program is distributed in the hope that it will be useful,
+ but WITHOUT ANY WARRANTY; without even the implied warranty of
+ MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ GNU General Public License for more details.
+
+ You should have received a copy of the GNU General Public License
+ along with this program. If not, see .
+'''
+
+'''
+Use example: ipython
+ run display.py -i
+'''
+
+import sys, getopt
+from helpers import Graphics
+
+def main(argv):
+ inputfile = ''
+ try:
+ opts, args = getopt.getopt(argv,"hi:",["ifile="])
+ except getopt.GetoptError:
+ print 'display.py -i '
+ sys.exit(2)
+
+ for opt, arg in opts:
+ if opt == '-h':
+ print 'display.py -i '
+ sys.exit()
+ elif opt in ("-i", "--ifile"):
+ inputfile = arg
+
+ motion = Graphics()
+ motion.show_motion(inputfile)
+
+if __name__ == "__main__":
+ main(sys.argv[1:])
diff --git a/Addition/PythonPlotter/plot_multiple.py b/Addition/PythonPlotter/PlotMultiple.py
similarity index 100%
rename from Addition/PythonPlotter/plot_multiple.py
rename to Addition/PythonPlotter/PlotMultiple.py
diff --git a/Addition/PythonPlotter/example.py b/Addition/PythonPlotter/example.py
deleted file mode 100644
index d6ba2df8..00000000
--- a/Addition/PythonPlotter/example.py
+++ /dev/null
@@ -1,320 +0,0 @@
-import numpy as np
-import matplotlib
-matplotlib.use('TkAgg')
-import matplotlib.pyplot as plt
-import os
-
-# Plot configuration
-PLOT_VERTICALLY = 0
-PLOT_HORIZONTALLY = 1
-
-# number of figures in this plot
-num_figures = 2
-
-def create_figures(subfigure_width=480, subfigure_height=600, starting_figure_no=1, starting_col_index = 0, starting_row_index=0, plot_configuration=PLOT_HORIZONTALLY):
- figure_number = starting_figure_no
- col_index = starting_col_index
- row_index = starting_row_index
-
-
- file_path = os.getcwd() + "/../../experiment_data_check/"
-
-
- # data_minj_pos_des = \
- # np.genfromtxt(file_path+'minj_pos.txt', delimiter=None, dtype=(float))
- # data_minj_vel_des = \
- # np.genfromtxt(file_path+'minj_vel.txt', delimiter=None, dtype=(float))
- # data_minj_acc_des = \
- # np.genfromtxt(file_path+'minj_acc.txt', delimiter=None, dtype=(float))
-
- ## read files
- data_rfoot_pos_des = \
- np.genfromtxt(file_path+'rfoot_pos_des.txt', delimiter=None, dtype=(float))
- data_rfoot_pos = \
- np.genfromtxt(file_path+'rfoot_pos.txt', delimiter=None, dtype=(float))
- data_lfoot_pos_des = \
- np.genfromtxt(file_path+'lfoot_pos_des.txt', delimiter=None, dtype=(float))
- data_lfoot_pos = \
- np.genfromtxt(file_path+'lfoot_pos.txt', delimiter=None, dtype=(float))
- data_jjpos_rfoot_pos = \
- np.genfromtxt(file_path+'jjpos_rfoot_pos.txt', delimiter=None, dtype=(float))
- data_jjpos_lfoot_pos = \
- np.genfromtxt(file_path+'jjpos_lfoot_pos.txt', delimiter=None, dtype=(float))
-
- data_LED = \
- np.genfromtxt(file_path+'LED_Pos.txt', delimiter=None, dtype=(float))
- data_LED_kin = \
- np.genfromtxt(file_path+'LED_Kin_Pos.txt', delimiter=None, dtype=(float))
- #data_foot_vel_des = \
- #np.genfromtxt(file_path+'rfoot_vel_des.txt', delimiter=None, dtype=(float))
- data_rfoot_vel = \
- np.genfromtxt(file_path+'rfoot_vel.txt', delimiter=None, dtype=(float))
- data_rfoot_acc_des = \
- np.genfromtxt(file_path+'rfoot_acc_des.txt', delimiter=None, dtype=(float))
- data_lfoot_acc_des = \
- np.genfromtxt(file_path+'lfoot_acc_des.txt', delimiter=None, dtype=(float))
-
-
- data_x = np.genfromtxt(file_path+'time.txt', delimiter='\n', dtype=(float))
- st_idx = 1
- end_idx = len(data_x) - 10
- data_x = data_x[st_idx:end_idx]
-
- rfoot_LED_idx = [6, 7];
- lfoot_LED_idx = [11, 12];
- data_LED_rfoot = ((data_LED[:, 3*rfoot_LED_idx[0]:3*rfoot_LED_idx[0]+3]) \
- + (data_LED[:, 3*rfoot_LED_idx[1]:3*rfoot_LED_idx[1]+3]))/2.;
-
- data_LED_lfoot = ((data_LED[:, 3*lfoot_LED_idx[0]:3*lfoot_LED_idx[0]+3]) \
- + (data_LED[:, 3*lfoot_LED_idx[1]:3*lfoot_LED_idx[1]+3]))/2.;
-
- data_LED_rfoot_out = (data_LED[:, 3*rfoot_LED_idx[0]:3*rfoot_LED_idx[0]+3])
- data_LED_lfoot_out = (data_LED[:, 3*lfoot_LED_idx[0]:3*lfoot_LED_idx[0]+3])
- data_LED_rfoot_in = (data_LED[:, 3*rfoot_LED_idx[1]:3*rfoot_LED_idx[1]+3])
- data_LED_lfoot_in = (data_LED[:, 3*lfoot_LED_idx[1]:3*lfoot_LED_idx[1]+3])
-
-
-
-
- # PHASE MARKER #
- data_phse = np.genfromtxt(file_path+'phase.txt', delimiter=None, dtype=(float))
- # get phase.txt data #
- phseChange = []
- for i in range(0,len(data_x)-1):
- if data_phse[i] != data_phse[i+1]:
- phseChange.append(i - st_idx)
- else:
- pass
-
- # LeftFoot Contact Signal #
- data_lf_contact = np.genfromtxt(file_path+'lfoot_contact.txt', delimiter=None, dtype=(float))
- data_lf_contact = data_lf_contact[st_idx:end_idx]
- lf_contact_index_change = []
- lf_contact_index_change.append(0)
- lf_contact_index_change.append(1)
- for i in range(2, len(data_x)):
- if data_lf_contact[i] != data_lf_contact[i-1]:
- lf_contact_index_change.append(i)
-
-
- # RightFoot Contact Signal #
- data_rf_contact = np.genfromtxt(file_path+'rfoot_contact.txt', delimiter=None, dtype=(float))
- data_rf_contact = data_rf_contact[st_idx:end_idx]
- rf_contact_index_change = []
- rf_contact_index_change.append(0)
- rf_contact_index_change.append(1)
- for i in range(2, len(data_x)):
- if data_rf_contact[i] != data_rf_contact[i-1]:
- rf_contact_index_change.append(i)
-
-
-
- axes = plt.gca()
-
- ## plot rfoot pos
- fig = plt.figure(figure_number)
- plt.get_current_fig_manager().window.wm_geometry(str(subfigure_width) + "x" + str(subfigure_height) + "+" + str(subfigure_width*col_index) + "+" + str(subfigure_height*row_index))
- fig.canvas.set_window_title('right foot pos')
- for i in range(1,4,1):
- ax1 = plt.subplot(3, 1, i)
- plt.plot(data_x, data_rfoot_pos_des[st_idx:end_idx,i-1], "r-")
- # plt.plot(data_x, data_minj_pos_des[st_idx:end_idx, i-1], color="black", linewidth=1.5)
- plt.plot(data_x, data_jjpos_rfoot_pos[st_idx:end_idx, i-1], color="black", linewidth=1.5)
- plt.plot(data_x, data_rfoot_pos[st_idx:end_idx,i-1], "b-")
-
- plt.plot(data_x, data_LED_rfoot[st_idx:end_idx, i-1] - data_LED_lfoot[st_idx:end_idx, i-1], \
- linewidth=2.0, color='orange')
- plt.plot(data_x, data_LED_rfoot_out[st_idx:end_idx, i-1] - data_LED_lfoot[st_idx:end_idx, i-1], \
- linewidth=1.0, color="indigo")
- plt.plot(data_x, data_LED_rfoot_in[st_idx:end_idx, i-1] - data_LED_lfoot[st_idx:end_idx, i-1], \
- linewidth=1.0, color="indigo")
- # plt.legend(('command', 'pos'), loc='upper left')
- # phase marker #
- for j in phseChange:
- # phase line
- plt.axvline(x=data_x[j],color='indigo',linestyle='-')
- # phase number
- plt.text(data_x[j],ax1.get_ylim()[1],'%d'%(data_phse[j]),color='indigo')
-
-
- # Plot Left Foot Contact
- for j in range(1, len(lf_contact_index_change)):
- t_start = data_x[lf_contact_index_change[j-1]]
- t_end = data_x[lf_contact_index_change[j]]
- y_start = data_lf_contact[lf_contact_index_change[j-1]] * ax1.get_ylim()[1]/2.0 * 0.9
- y_end = data_lf_contact[lf_contact_index_change[j]] * ax1.get_ylim()[1]/2.0 * 0.9
-
- # Plot Square Wave
- plt.plot([t_start, t_end, t_end], [y_start, y_start, y_end], color='orange')
- plt.text((t_start+(t_end-t_start)/2.0), y_start,'L%d'%(data_lf_contact[lf_contact_index_change[j-1]]),color='orange')
-
-
- # Plot Right Foot Contact
- for j in range(1, len(rf_contact_index_change)):
- t_start = data_x[rf_contact_index_change[j-1]]
- t_end = data_x[rf_contact_index_change[j]]
- y_start = data_rf_contact[rf_contact_index_change[j-1]] * ax1.get_ylim()[1]/2.0
- y_end = data_rf_contact[rf_contact_index_change[j]] * ax1.get_ylim()[1]/2.0
-
- # Plot Square Wave
- plt.plot([t_start, t_end, t_end], [y_start, y_start, y_end], color='magenta')
- plt.text((t_start+(t_end-t_start)/2.0), y_start,'R%d'%(data_rf_contact[rf_contact_index_change[j-1]]),color='magenta')
-
-
-
-
- plt.grid(True)
- plt.xlabel('time (sec)')
- ## increment figure number and index
- figure_number += 1
- if plot_configuration == PLOT_HORIZONTALLY:
- col_index += 1
- elif plot_configuration == PLOT_VERTICALLY:
- row_index +=1
-
- ## plot lfoot pos
- fig = plt.figure(figure_number)
- plt.get_current_fig_manager().window.wm_geometry(str(subfigure_width) + "x" + str(subfigure_height) + "+" + str(subfigure_width*col_index) + "+" + str(subfigure_height*row_index))
- fig.canvas.set_window_title('left foot pos')
- for i in range(1,4,1):
- ax1 = plt.subplot(3, 1, i)
- plt.plot(data_x, data_lfoot_pos_des[st_idx:end_idx,i-1], "r-")
- plt.plot(data_x, data_jjpos_lfoot_pos[st_idx:end_idx, i-1], color="black", linewidth=1.5)
- plt.plot(data_x, data_lfoot_pos[st_idx:end_idx,i-1], "b-")
-
- plt.plot(data_x, data_LED_lfoot[st_idx:end_idx, i-1] \
- - data_LED_rfoot[st_idx:end_idx, i-1], \
- linewidth=2.0, color='orange')
-
- plt.plot(data_x, data_LED_lfoot_out[st_idx:end_idx, i-1] \
- - data_LED_rfoot[st_idx:end_idx, i-1], \
- linewidth=1.0, color="indigo")
-
- plt.plot(data_x, data_LED_lfoot_in[st_idx:end_idx, i-1] \
- - data_LED_rfoot[st_idx:end_idx, i-1], \
- linewidth=1.0, color="indigo")
- # plt.legend(('command', 'pos'), loc='upper left')
- # phase marker #
- for j in phseChange:
- # phase line
- plt.axvline(x=data_x[j],color='indigo',linestyle='-')
- # phase number
- plt.text(data_x[j],ax1.get_ylim()[1],'%d'%(data_phse[j]),color='indigo')
-
-
- # Plot Left Foot Contact
- for j in range(1, len(lf_contact_index_change)):
- t_start = data_x[lf_contact_index_change[j-1]]
- t_end = data_x[lf_contact_index_change[j]]
- y_start = data_lf_contact[lf_contact_index_change[j-1]] * ax1.get_ylim()[1]/2.0 * 0.9
- y_end = data_lf_contact[lf_contact_index_change[j]] * ax1.get_ylim()[1]/2.0 * 0.9
-
- # Plot Square Wave
- plt.plot([t_start, t_end, t_end], [y_start, y_start, y_end], color='orange')
- plt.text((t_start+(t_end-t_start)/2.0), y_start,'L%d'%(data_lf_contact[lf_contact_index_change[j-1]]),color='orange')
-
-
- # Plot Right Foot Contact
- for j in range(1, len(rf_contact_index_change)):
- t_start = data_x[rf_contact_index_change[j-1]]
- t_end = data_x[rf_contact_index_change[j]]
- y_start = data_rf_contact[rf_contact_index_change[j-1]] * ax1.get_ylim()[1]/2.0
- y_end = data_rf_contact[rf_contact_index_change[j]] * ax1.get_ylim()[1]/2.0
-
- # Plot Square Wave
- plt.plot([t_start, t_end, t_end], [y_start, y_start, y_end], color='magenta')
- plt.text((t_start+(t_end-t_start)/2.0), y_start,'R%d'%(data_rf_contact[rf_contact_index_change[j-1]]),color='magenta')
-
-
-
-
- plt.grid(True)
- plt.xlabel('time (sec)')
- ## increment figure number and index
- figure_number += 1
- if plot_configuration == PLOT_HORIZONTALLY:
- col_index += 1
- elif plot_configuration == PLOT_VERTICALLY:
- row_index +=1
-
-
- ## plot foot vel
- fig = plt.figure(figure_number)
- plt.get_current_fig_manager().window.wm_geometry(str(subfigure_width) + "x" + str(subfigure_height) + "+" + str(subfigure_width*col_index) + "+" + str(subfigure_height*row_index))
- fig.canvas.set_window_title('foot vel')
- for i in range(1,4,1):
- ax1 = plt.subplot(3, 1, i)
- #plt.plot(data_x, data_foot_vel_des[st_idx:end_idx,i-1], "r-", \
- # data_x, data_rfoot_vel[st_idx:end_idx,i-1], "b-")
- plt.plot(data_x, data_rfoot_vel[st_idx:end_idx,i-1], "b-")
- # plt.plot(data_x, data_minj_vel_des[st_idx:end_idx, i-1], color="black", linewidth=1.5)
-
- # plt.legend(('command', 'pos'), loc='upper left')
- # phase marker #
- for j in phseChange:
- # phase line
- plt.axvline(x=data_x[j],color='indigo',linestyle='-')
- # phase number
- plt.text(data_x[j],ax1.get_ylim()[1],'%d'%(data_phse[j]),color='indigo')
- plt.grid(True)
- plt.xlabel('time (sec)')
- ## increment figure number and index
- figure_number += 1
- if plot_configuration == PLOT_HORIZONTALLY:
- col_index += 1
- elif plot_configuration == PLOT_VERTICALLY:
- row_index +=1
-
- ## plot foot acc
- fig = plt.figure(figure_number)
- plt.get_current_fig_manager().window.wm_geometry(str(subfigure_width) + "x" + str(subfigure_height) + "+" + str(subfigure_width*col_index) + "+" + str(subfigure_height*row_index))
- fig.canvas.set_window_title('rfoot acc')
- for i in range(1,4,1):
- ax1 = plt.subplot(3, 1, i)
- plt.plot(data_x, data_rfoot_acc_des[st_idx:end_idx,i-1], "r-")
- # plt.plot(data_x, data_minj_acc_des[st_idx:end_idx, i-1], color="black", linewidth=1.5)
-
- # phase marker #
- for j in phseChange:
- # phase line
- plt.axvline(x=data_x[j],color='indigo',linestyle='-')
- # phase number
- plt.text(data_x[j],ax1.get_ylim()[1],'%d'%(data_phse[j]),color='indigo')
- plt.grid(True)
- plt.xlabel('time (sec)')
- ## increment figure number and index
- figure_number += 1
- if plot_configuration == PLOT_HORIZONTALLY:
- col_index += 1
- elif plot_configuration == PLOT_VERTICALLY:
- row_index +=1
-
- ## plot foot acc
- fig = plt.figure(figure_number)
- plt.get_current_fig_manager().window.wm_geometry(str(subfigure_width) + "x" + str(subfigure_height) + "+" + str(subfigure_width*col_index) + "+" + str(subfigure_height*row_index))
- fig.canvas.set_window_title('lfoot acc')
- for i in range(1,4,1):
- ax1 = plt.subplot(3, 1, i)
- plt.plot(data_x, data_lfoot_acc_des[st_idx:end_idx,i-1], "r-")
- # phase marker #
- for j in phseChange:
- # phase line
- plt.axvline(x=data_x[j],color='indigo',linestyle='-')
- # phase number
- plt.text(data_x[j],ax1.get_ylim()[1],'%d'%(data_phse[j]),color='indigo')
- plt.grid(True)
- plt.xlabel('time (sec)')
- ## increment figure number and index
- figure_number += 1
- if plot_configuration == PLOT_HORIZONTALLY:
- col_index += 1
- elif plot_configuration == PLOT_VERTICALLY:
- row_index +=1
-
-
-
-
-
-if __name__ == "__main__":
- create_figures()
- plt.show()
diff --git a/Addition/PythonPlotter/helpers.py b/Addition/PythonPlotter/helpers.py
new file mode 100644
index 00000000..6337d6c6
--- /dev/null
+++ b/Addition/PythonPlotter/helpers.py
@@ -0,0 +1,377 @@
+'''
+ Copyright [2017] Max Planck Society. All rights reserved.
+
+ This program is free software: you can redistribute it and/or modify
+ it under the terms of the GNU General Public License as published by
+ the Free Software Foundation, either version 3 of the License, or
+ (at your option) any later version.
+
+ This program is distributed in the hope that it will be useful,
+ but WITHOUT ANY WARRANTY; without even the implied warranty of
+ MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ GNU General Public License for more details.
+
+ You should have received a copy of the GNU General Public License
+ along with this program. If not, see .
+'''
+
+import numpy as np
+from scipy import signal
+import yaml, math, matplotlib
+import matplotlib.pyplot as plt
+import matplotlib.patches as patches
+import matplotlib.gridspec as gridspec
+from mpl_toolkits.mplot3d import Axes3D
+
+'Conversion from quaternion to rotation matrix'
+def quat2mat(quat):
+ quat = np.squeeze(np.asarray(quat))
+ w, x, y, z = quat
+ return np.matrix([
+ [1 - 2*y*y-2*z*z, 2*x*y - 2*z*w, 2*x*z+2*y*w],
+ [2*x*y+2*z*w, 1-2*x*x-2*z*z, 2*y*z-2*x*w],
+ [2*x*z-2*y*w, 2*y*z+2*x*w, 1-2*x*x-2*y*y]])
+
+'Base class to define a contact state'
+class CntState:
+ def __init__(self, pars):
+ self.tini = pars[0]
+ self.tend = pars[1]
+ self.pos = np.matrix(pars[2:5]).transpose()
+ self.ori = quat2mat(pars[5:9])
+
+ def display(self):
+ print "time", self.tini, "--", self.tend
+ print "pos ", self.pos
+ print "ori ", self.ori
+ print "---------------------------------"
+
+'Base class to show motion'
+class Graphics:
+ 'Initialization'
+ def __init__(self):
+ self.effs_cnts = list()
+
+ 'Helper functions'
+ def id_to_eff_str_map(self, id):
+ if (id == 0):
+ return 'rf'
+ elif (id == 1):
+ return 'lf'
+ elif (id == 2):
+ return 'rh'
+ elif (id == 3):
+ return 'lh'
+ else:
+ print 'Err: id not handled'
+
+ def nice_timesteps_plot(self, axis, timesteps1, timesteps2):
+ axis.plot(timesteps1, color='royalblue')
+ axis.plot(timesteps2, color='lightseagreen')
+
+ shading = 0.2
+ tick_size = 10
+ grid_color = '#ACADA9'
+ axis_color = '#8C8E87'
+ tick_color = '#434440'
+ ylim = [0.05, 0.30]
+ xlim = [1,len(timesteps1)-1]
+ linecolors = {0: 'cornflowerblue', 1: 'darkorange', 2: 'seagreen', 3: 'gold'}
+
+ time_spec = np.squeeze(np.asarray(np.linspace(1, len(timesteps1), len(timesteps1))))
+ time_vec = np.squeeze(np.asarray(np.linspace(self.time_step, self.time_horizon, len(timesteps1))))
+ for eef_id in range(0,4):
+ trueData = np.zeros(len(timesteps1))
+ for cnt_id in range(0,len(self.effs_cnts[eef_id])):
+ for id in range(0,len(trueData)):
+ if (time_vec[id] >= self.effs_cnts[eef_id][cnt_id].tini and time_vec[id] < self.effs_cnts[eef_id][cnt_id].tend):
+ trueData[id] = 1
+ axis.fill_between(time_spec, ylim[0], ylim[1], where=trueData>0.5, facecolor=linecolors[eef_id], alpha=shading)
+
+ axis.grid(True)
+ axis.set_xlim(xlim)
+ axis.set_ylim(ylim)
+ for item in axis.get_xgridlines():
+ item.set_color(grid_color)
+ for item in axis.get_ygridlines():
+ item.set_color(grid_color)
+
+ axis.yaxis.tick_right()
+ for t in axis.axes.get_yticklabels():
+ t.set_horizontalalignment('right')
+ t.set_x(1.04)
+ axis.spines["top"].set_color(axis_color)
+ axis.spines["left"].set_color(axis_color)
+ axis.spines["right"].set_color(axis_color)
+ axis.spines["bottom"].set_color(axis_color)
+ axis.tick_params(labelsize=tick_size, colors=tick_color)
+ axis.set_ylabel(r'$\Delta t$', fontsize=10, color='#373834')
+
+ def nice_mom_plot(self, axis, datax, datay, line_color, line_width, line_style, grid_color, axis_color, xlim, ylim, xmsg, ymsg, label_size, label_color, show_xlabel, tick_size, tick_color, remove_Xticks, remove_Yticks, linecolors, shading, linelabel, showLabel):
+ axis.plot(datax, datay, color=line_color, linewidth=line_width, linestyle=line_style, label=linelabel)
+
+ if (showLabel is True):
+ leg = axis.legend(bbox_to_anchor=(0., -0.30, 1., .102), loc=3, ncol=2, mode="expand", borderaxespad=0.)
+
+ time_vec = np.squeeze(np.asarray(np.linspace(self.time_step, self.time_horizon, len(datax))))
+ for eef_id in range(0,4):
+ trueData = np.zeros(len(datax))
+ for cnt_id in range(0,len(self.effs_cnts[eef_id])):
+ for id in range(0,len(trueData)):
+ if (time_vec[id] >= self.effs_cnts[eef_id][cnt_id].tini and time_vec[id] < self.effs_cnts[eef_id][cnt_id].tend):
+ trueData[id] = 1
+ axis.fill_between(datax, ylim[0], ylim[1], where=trueData>0.5, facecolor=linecolors[eef_id], alpha=shading)
+
+ axis.grid(True)
+ axis.set_xlim(xlim)
+ axis.set_ylim(ylim)
+ for item in axis.get_xgridlines():
+ item.set_color(grid_color)
+ for item in axis.get_ygridlines():
+ item.set_color(grid_color)
+ if (remove_Xticks is True):
+ axis.axes.xaxis.set_ticklabels([])
+ if (remove_Yticks is True):
+ axis.axes.yaxis.set_ticklabels([])
+ else:
+ axis.yaxis.tick_right()
+ for t in axis.axes.get_yticklabels():
+ t.set_horizontalalignment('right')
+ t.set_x(1.15)
+
+ axis.spines["top"].set_color(axis_color)
+ axis.spines["left"].set_color(axis_color)
+ axis.spines["right"].set_color(axis_color)
+ axis.spines["bottom"].set_color(axis_color)
+ axis.tick_params(labelsize=tick_size, colors=tick_color)
+ axis.set_ylabel(ymsg, fontsize=label_size, color=label_color)
+ if (show_xlabel is True):
+ axis.set_xlabel(xmsg, fontsize=label_size, color=label_color)
+
+ def nice_frc_plot(self, axis, datax, datay, linecolors, linestyles, linelabels, addlabels, line_width, grid_color, tick_size, tick_color, axis_color, xlim, ylim, xmsg, ymsg, label_size, label_color, legend_textsize, legend_color, legend_fcolor, legend_flinewidth, legend_fbgcolor, shading):
+ axis.grid(True)
+ axis.set_xlim(xlim)
+ axis.set_ylim(ylim)
+ for item in axis.get_xgridlines():
+ item.set_color(grid_color)
+ for item in axis.get_ygridlines():
+ item.set_color(grid_color)
+ axis.spines["top"].set_color(axis_color)
+ axis.spines["left"].set_color(axis_color)
+ axis.spines["right"].set_color(axis_color)
+ axis.spines["bottom"].set_color(axis_color)
+ axis.tick_params(labelsize=tick_size, colors=tick_color)
+ axis.set_xlabel(xmsg, fontsize=label_size, color=label_color)
+ axis.set_ylabel(ymsg, fontsize=label_size, color=label_color)
+ axis.yaxis.tick_right()
+ for t in axis.axes.get_yticklabels():
+ t.set_horizontalalignment('right')
+ t.set_x(1.15)
+ if (addlabels is True):
+ axis.axes.xaxis.set_ticklabels([])
+
+ time_vec = np.squeeze(np.asarray(np.linspace(self.time_step, self.time_horizon, len(datax))))
+ for eef_id in range(0,4):
+ trueData = np.zeros(len(datax))
+ for cnt_id in range(0,len(self.effs_cnts[eef_id])):
+ for id in range(0,len(trueData)):
+ if (time_vec[id] >= self.effs_cnts[eef_id][cnt_id].tini and time_vec[id] < self.effs_cnts[eef_id][cnt_id].tend):
+ trueData[id] = 1
+ axis.fill_between(datax, ylim[0], ylim[1], where=trueData>0.5, facecolor=linecolors[eef_id], alpha=shading)
+
+
+ for id in range(0, len(datay)):
+ if (addlabels is False):
+ axis.plot(datax, datay[id], color=linecolors[id], linewidth=line_width, linestyle=linestyles[id])
+ else:
+ axis.plot(datax, datay[id], label=linelabels[id], color=linecolors[id], linewidth=line_width, linestyle=linestyles[id])
+
+ if (addlabels is True):
+ leg = axis.legend(bbox_to_anchor=(0., -0.30, 1., .102), loc=3, ncol=4, mode="expand", borderaxespad=0.)
+
+ 'Function to show the motion'
+ def show_motion(self, cfg_file):
+ 'Read data from file'
+ with open(cfg_file, 'r') as stream:
+ try:
+ cfg_pars = yaml.load(stream)
+ self.time_step = cfg_pars['dynopt_params']['time_step']
+ self.robot_mass = cfg_pars['dynopt_params']['robot_mass']
+ self.n_act_eefs = cfg_pars['dynopt_params']['n_act_eefs']
+ self.time_horizon = cfg_pars['dynopt_params']['time_horizon']
+ self.time_vec = np.matrix(cfg_pars['dynopt_params']['time_vec'])
+
+ self.com = np.matrix(cfg_pars['dynopt_params']['com_motion'])
+ self.com_ref = np.matrix(cfg_pars['dynopt_params']['com_motion_ref'])
+ self.lmom = np.matrix(cfg_pars['dynopt_params']['lin_mom'])/self.robot_mass
+ self.amom = np.matrix(cfg_pars['dynopt_params']['ang_mom'])/self.robot_mass
+ self.lmom_ref = np.matrix(cfg_pars['dynopt_params']['lin_mom_ref'])/self.robot_mass
+ self.amom_ref = np.matrix(cfg_pars['dynopt_params']['ang_mom_ref'])/self.robot_mass
+ self.eef_frcs = list()
+ for eff_id in range(0, self.n_act_eefs):
+ self.eef_frcs.insert(eff_id, np.matrix(cfg_pars['dynopt_params']['eef_frc_'+str(eff_id)]))
+
+ for eff_id in range(0, 4):
+ eef_cnts = list()
+ for cnt_id in range(0, cfg_pars['cntopt_params']['num_contacts'][eff_id]):
+ eef_cnts.append(CntState(cfg_pars['cntopt_params']['eefcnt_'+self.id_to_eff_str_map(eff_id)]['cnt'+str(cnt_id)]))
+ self.effs_cnts.append(eef_cnts)
+
+ except yaml.YAMLError as exc:
+ print(exc)
+
+ 'Build arrays of data to be displayed'
+ comx = np.squeeze(np.asarray(self.com[0,:]))
+ comy = np.squeeze(np.asarray(self.com[1,:]))
+ comz = np.squeeze(np.asarray(self.com[2,:]))
+ lmomx = np.squeeze(np.asarray(self.lmom[0,:]))
+ lmomy = np.squeeze(np.asarray(self.lmom[1,:]))
+ lmomz = np.squeeze(np.asarray(self.lmom[2,:]))
+ amomx = np.squeeze(np.asarray(self.amom[0,:]))
+ amomy = np.squeeze(np.asarray(self.amom[1,:]))
+ amomz = np.squeeze(np.asarray(self.amom[2,:]))
+
+ comrefx = np.squeeze(np.asarray(self.com_ref[0,:]))
+ comrefy = np.squeeze(np.asarray(self.com_ref[1,:]))
+ comrefz = np.squeeze(np.asarray(self.com_ref[2,:]))
+ lmomrefx = np.squeeze(np.asarray(self.lmom_ref[0,:]))
+ lmomrefy = np.squeeze(np.asarray(self.lmom_ref[1,:]))
+ lmomrefz = np.squeeze(np.asarray(self.lmom_ref[2,:]))
+ amomrefx = np.squeeze(np.asarray(self.amom_ref[0,:]))
+ amomrefy = np.squeeze(np.asarray(self.amom_ref[1,:]))
+ amomrefz = np.squeeze(np.asarray(self.amom_ref[2,:]))
+
+ time = np.squeeze(np.asarray(self.time_vec[0,:]))
+ timesteps1 = np.squeeze(np.asarray( np.zeros((len(time),1)) ))
+ timesteps2 = np.squeeze(np.asarray( np.zeros((len(time),1)) ))
+
+ for id in range (0,len(time)):
+ timesteps1[id] = self.time_step
+ if id is 0:
+ timesteps2[id] = time[id]
+ else:
+ timesteps2[id] = time[id]-time[id-1]
+
+ frcs_x = list()
+ frcs_y = list()
+ frcs_z = list()
+ for eff_id in range(0,self.n_act_eefs):
+ frcs_x.insert(eff_id, np.squeeze(np.asarray(self.eef_frcs[eff_id][0,:])))
+ frcs_y.insert(eff_id, np.squeeze(np.asarray(self.eef_frcs[eff_id][1,:])))
+ frcs_z.insert(eff_id, np.squeeze(np.asarray(self.eef_frcs[eff_id][2,:])))
+
+ 'Figure1: Center of Mass motion'
+ offset = 0.05
+ com_linewidth = 2
+ comref_linewidth = 2
+ com_axis_tick_size = 10
+ com_axis_label_size = 14
+ com_axis_tick_color = '#434440'
+ com_axis_label_color = '#373834'
+ com_linecolor = 'cornflowerblue'
+ comref_linecolor = 'darkorange'
+ linestyles = {0:'--', 1:'-', 2: '--', 3: '-'}
+ endeffectors = {0: 'RF', 1: 'LF', 2: 'RH', 3: 'LH'}
+ colors = {0: 'red', 1: 'magenta', 2: 'blue', 3: 'cyan'}
+ linecolors = {0: 'cornflowerblue', 1: 'sandybrown', 2: 'seagreen', 3: 'gold'}
+
+ 'Figure1: Center of mass motion'
+ fig1 = plt.figure()
+ CoM_motion = Axes3D(fig1)
+ CoM_motion.plot(xs=comx, ys=comy, zs=comz, linewidth=com_linewidth, color=com_linecolor)
+ CoM_motion.plot(xs=comrefx, ys=comrefy, zs=comrefz, linewidth=comref_linewidth, color=comref_linecolor, linestyle='--')
+ for eff_id in range(0, 4):
+ for cnt_id in range(0, len(self.effs_cnts[eff_id])):
+ cnt_pos = self.effs_cnts[eff_id][cnt_id].pos
+ cnt_pos = np.squeeze(np.asarray(cnt_pos))
+ cnt_ori = self.effs_cnts[eff_id][cnt_id].ori[:,2]
+ CoM_motion.text(cnt_pos[0]+offset, cnt_pos[1]+offset, cnt_pos[2]+offset, endeffectors[eff_id]+str(cnt_id), color=colors[eff_id])
+
+ point = np.array([cnt_pos[0], cnt_pos[1], cnt_pos[2]])
+ normal = np.array([cnt_ori[0,0], cnt_ori[1,0], cnt_ori[2,0]])
+ d = -point.dot(normal)
+ xx, yy = np.meshgrid(np.linspace(cnt_pos[0]-0.05,cnt_pos[0]+0.05,2), np.linspace(cnt_pos[1]-0.1,cnt_pos[1]+0.1,2))
+ z = (-normal[0] * xx - normal[1] * yy - d) * 1. /normal[2]
+ CoM_motion.plot_wireframe(xx, yy, z, color=colors[eff_id], linewidth=1.5)
+ CoM_motion.plot_surface(xx, yy, z, edgecolors=colors[eff_id], color=colors[eff_id], alpha = 0.5)
+ CoM_motion.scatter(xs=cnt_pos[0], ys=cnt_pos[1], zs=cnt_pos[2], zdir='z', s=50, c=colors[eff_id], depthshade=True)
+
+ CoM_motion.tick_params(labelsize=com_axis_tick_size, colors=com_axis_tick_color)
+ CoM_motion.set_xlabel('Lateral direction', fontsize=com_axis_label_size, color=com_axis_label_color)
+ CoM_motion.set_ylabel('Forward direction', fontsize=com_axis_label_size, color=com_axis_label_color)
+ CoM_motion.set_zlabel('Vertical direction', fontsize=com_axis_label_size, color=com_axis_label_color)
+
+ 'Figure2: Linear and angular momenta, forces and timesteps'
+ mom_line_width = 2
+ momref_line_width = 2
+ mom_axis_tick_size = 10
+ mom_axis_label_size = 10
+
+ lmomx_y_size = [-50.0/self.robot_mass, 50.0/self.robot_mass]
+ lmomy_y_size = [-25.0/self.robot_mass, 25.0/self.robot_mass]
+ lmomz_y_size = [-25.0/self.robot_mass, 25.0/self.robot_mass]
+ amom_y_size = [-10.0/self.robot_mass, 10.0/self.robot_mass]
+ mom_x_size = [1.0, time[len(time)-1]]
+ mom_axis_color = '#8C8E87'
+ mom_grid_color = '#ACADA9'
+ mom_axis_tick_color = '#434440'
+ mom_axis_label_color = '#373834'
+ mom_line_color = 'cornflowerblue'
+ momref_line_color = 'darkorange'
+ mom_line_styles = {0:'-', 1:'--'}
+ shading = 0.1
+
+ frc_line_width = 2
+ frc_axis_tick_size = 10
+ frc_legend_textsize = 10
+ frc_axis_label_size = 10
+ frc_x_ysize = [-0.40, 0.40]
+ frc_y_ysize = [-0.20, 0.20]
+ frc_z_ysize = [-0.01, 1.02]
+ frc_legend_frame_linewidth = 1
+ frc_xsize = [1.0, time[len(time)-1]]
+ frc_grid_color = '#ACADA9'
+ frc_axis_color = '#8C8E87'
+ frc_legend_color = '#8C8E87'
+ frc_axis_tick_color = '#434440'
+ frc_axis_label_color = '#373834'
+ frc_legend_frame_color = '#ACADA9'
+ frc_legend_frame_background_color = '#EAECEF'
+
+ fig1 = plt.figure(figsize=(10,5))
+ gs = gridspec.GridSpec(4, 3)
+
+ lmom_x1 = plt.subplot(gs[0,0])
+ lmom_y1 = plt.subplot(gs[0,1])
+ lmom_z1 = plt.subplot(gs[0,2])
+ amom_x1 = plt.subplot(gs[1,0])
+ amom_y1 = plt.subplot(gs[1,1])
+ amom_z1 = plt.subplot(gs[1,2])
+ frc_x1 = plt.subplot(gs[2,0])
+ frc_y1 = plt.subplot(gs[2,1])
+ frc_z1 = plt.subplot(gs[2,2])
+ tplt_1 = plt.subplot(gs[3,:])
+
+ self.nice_mom_plot(lmom_x1, time, lmomx, mom_line_color, mom_line_width, mom_line_styles[0], mom_grid_color, mom_axis_color, mom_x_size, lmomx_y_size, r"""Time $[sec]$""", r"""LinX""", mom_axis_label_size, mom_axis_label_color, False, mom_axis_tick_size, mom_axis_tick_color, True, False, linecolors, shading, linelabel='DynMom', showLabel=False)
+ self.nice_mom_plot(lmom_y1, time, lmomy, mom_line_color, mom_line_width, mom_line_styles[0], mom_grid_color, mom_axis_color, mom_x_size, lmomy_y_size, r"""Time $[sec]$""", r"""LinY""", mom_axis_label_size, mom_axis_label_color, False, mom_axis_tick_size, mom_axis_tick_color, True, False, linecolors, shading, linelabel='DynMom', showLabel=False)
+ self.nice_mom_plot(lmom_z1, time, lmomz, mom_line_color, mom_line_width, mom_line_styles[0], mom_grid_color, mom_axis_color, mom_x_size, lmomz_y_size, r"""Time $[sec]$""", r"""LinZ""", mom_axis_label_size, mom_axis_label_color, False, mom_axis_tick_size, mom_axis_tick_color, True, False, linecolors, shading, linelabel='DynMom', showLabel=False)
+ self.nice_mom_plot(amom_x1, time, amomx, mom_line_color, mom_line_width, mom_line_styles[0], mom_grid_color, mom_axis_color, mom_x_size, amom_y_size, r"""Time $[sec]$""", r"""AngX""", mom_axis_label_size, mom_axis_label_color, False, mom_axis_tick_size, mom_axis_tick_color, True, False, linecolors, shading, linelabel='DynMom', showLabel=False)
+ self.nice_mom_plot(amom_y1, time, amomy, mom_line_color, mom_line_width, mom_line_styles[0], mom_grid_color, mom_axis_color, mom_x_size, amom_y_size, r"""Time $[sec]$""", r"""AngY""", mom_axis_label_size, mom_axis_label_color, False, mom_axis_tick_size, mom_axis_tick_color, True, False, linecolors, shading, linelabel='DynMom', showLabel=False)
+ self.nice_mom_plot(amom_z1, time, amomz, mom_line_color, mom_line_width, mom_line_styles[0], mom_grid_color, mom_axis_color, mom_x_size, amom_y_size, r"""Time $[sec]$""", r"""AngZ""", mom_axis_label_size, mom_axis_label_color, False, mom_axis_tick_size, mom_axis_tick_color, True, False, linecolors, shading, linelabel='DynMom', showLabel=False)
+
+ self.nice_mom_plot(lmom_x1, time, lmomrefx, momref_line_color, momref_line_width, mom_line_styles[1], mom_grid_color, mom_axis_color, mom_x_size, lmomx_y_size, r"""Time $[sec]$""", r"""LinX""", mom_axis_label_size, mom_axis_label_color, False, mom_axis_tick_size, mom_axis_tick_color, True, False, linecolors, shading, linelabel='KinMom', showLabel=True)
+ self.nice_mom_plot(lmom_y1, time, lmomrefy, momref_line_color, momref_line_width, mom_line_styles[1], mom_grid_color, mom_axis_color, mom_x_size, lmomy_y_size, r"""Time $[sec]$""", r"""LinY""", mom_axis_label_size, mom_axis_label_color, False, mom_axis_tick_size, mom_axis_tick_color, True, False, linecolors, shading, linelabel='KinMom', showLabel=False)
+ self.nice_mom_plot(lmom_z1, time, lmomrefz, momref_line_color, momref_line_width, mom_line_styles[1], mom_grid_color, mom_axis_color, mom_x_size, lmomz_y_size, r"""Time $[sec]$""", r"""LinZ""", mom_axis_label_size, mom_axis_label_color, False, mom_axis_tick_size, mom_axis_tick_color, True, False, linecolors, shading, linelabel='KinMom', showLabel=False)
+ self.nice_mom_plot(amom_x1, time, amomrefx, momref_line_color, momref_line_width, mom_line_styles[1], mom_grid_color, mom_axis_color, mom_x_size, amom_y_size, r"""Time $[sec]$""", r"""AngX""", mom_axis_label_size, mom_axis_label_color, False, mom_axis_tick_size, mom_axis_tick_color, True, False, linecolors, shading, linelabel='KinMom', showLabel=True)
+ self.nice_mom_plot(amom_y1, time, amomrefy, momref_line_color, momref_line_width, mom_line_styles[1], mom_grid_color, mom_axis_color, mom_x_size, amom_y_size, r"""Time $[sec]$""", r"""AngY""", mom_axis_label_size, mom_axis_label_color, False, mom_axis_tick_size, mom_axis_tick_color, True, False, linecolors, shading, linelabel='KinMom', showLabel=False)
+ self.nice_mom_plot(amom_z1, time, amomrefz, momref_line_color, momref_line_width, mom_line_styles[1], mom_grid_color, mom_axis_color, mom_x_size, amom_y_size, r"""Time $[sec]$""", r"""AngZ""", mom_axis_label_size, mom_axis_label_color, False, mom_axis_tick_size, mom_axis_tick_color, True, False, linecolors, shading, linelabel='KinMom', showLabel=False)
+
+ self.nice_frc_plot(frc_x1, time, frcs_x, linecolors, linestyles, endeffectors, True, frc_line_width, frc_grid_color, frc_axis_tick_size, frc_axis_tick_color, frc_axis_color, frc_xsize, frc_x_ysize, r"""""", r"""FrcX""", frc_axis_label_size, frc_axis_label_color, frc_legend_textsize, frc_legend_color, frc_legend_frame_color, frc_legend_frame_linewidth, frc_legend_frame_background_color, shading)
+ self.nice_frc_plot(frc_y1, time, frcs_y, linecolors, linestyles, endeffectors, False, frc_line_width, frc_grid_color, frc_axis_tick_size, frc_axis_tick_color, frc_axis_color, frc_xsize, frc_y_ysize, r"""""", r"""FrcY""", frc_axis_label_size, frc_axis_label_color, frc_legend_textsize, frc_legend_color, frc_legend_frame_color, frc_legend_frame_linewidth, frc_legend_frame_background_color, shading)
+ self.nice_frc_plot(frc_z1, time, frcs_z, linecolors, linestyles, endeffectors, False, frc_line_width, frc_grid_color, frc_axis_tick_size, frc_axis_tick_color, frc_axis_color, frc_xsize, frc_z_ysize, r"""""", r"""FrcZ""", frc_axis_label_size, frc_axis_label_color, frc_legend_textsize, frc_legend_color, frc_legend_frame_color, frc_legend_frame_linewidth, frc_legend_frame_background_color, shading)
+
+ tplt_1.set_title("Timesteps", fontsize=12, color=frc_axis_label_color)
+ self.nice_timesteps_plot(tplt_1, timesteps1, timesteps2)
+
+ fig1.subplots_adjust(left=0.03, bottom=0.08, right=0.94, top=0.95, wspace=0.36, hspace=0.65)
+ plt.show()
diff --git a/CMakeLists.txt b/CMakeLists.txt
index 348f49d5..7dd14c7d 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -57,6 +57,7 @@ include_directories("${PROJECT_SOURCE_DIR}/PnC/Trajectory")
include_directories("${PROJECT_SOURCE_DIR}/ExternalSource/Yaml/include")
include_directories("${PROJECT_SOURCE_DIR}/ExternalSource")
include_directories("${PROJECT_SOURCE_DIR}/ExternalSource/Optimizer/")
+include_directories("${PROJECT_SOURCE_DIR}/ExternalSource/Optimizer/ConicSolver/include")
include_directories("${PROJECT_SOURCE_DIR}/Geometry/Polytope")
## ================
diff --git a/Config/Draco/CENTROIDPLANNER.yaml b/Config/Draco/CENTROID_PLANNER.yaml
similarity index 92%
rename from Config/Draco/CENTROIDPLANNER.yaml
rename to Config/Draco/CENTROID_PLANNER.yaml
index 919e7806..b246ad35 100644
--- a/Config/Draco/CENTROIDPLANNER.yaml
+++ b/Config/Draco/CENTROID_PLANNER.yaml
@@ -12,7 +12,7 @@ initial_robot_configuration:
eef_frc_lf: [0.0, 0.0, 0.5]
eef_frc_rh: [0.0, 0.0, 0.0]
eef_frc_lh: [0.0, 0.0, 0.0]
- eef_pose:
+ eef_pose: # activation, pos, quat
eef_rf: [1.0, 0.086, 0.15, -0.92, 1.0, 0.0, 0.0, 0.0]
eef_lf: [1.0, -0.086, 0.15, -0.92, 1.0, 0.0, 0.0, 0.0]
eef_rh: [0.0, 0.400, 0.30, 0.00, 0.0, -0.70711, 0.70711, 0.0 ]
@@ -26,16 +26,16 @@ contact_plan:
######################
num_contacts: [3, 3, 0, 0]
- effcnt_rf:
+ eefcnt_rf:
cnt0: [0.0, 1.0, 0.086, 0.15, -0.92, 1.0, 0.0, 0.0, 0.0, 1.0]
cnt1: [2.0, 4.5, 0.500, 0.45, -0.76, 0.9689, 0.0, -0.2474, 0.0, 1.0]
cnt2: [6.0, 9.9, 0.450, 0.98, -0.27, 1.0, 0.0, 0.0, 0.0, 1.0]
- effcnt_lf:
+ eefcnt_lf:
cnt0: [0.0, 2.5, -0.086, 0.15, -0.92, 1.0, 0.0, 0.0, 0.0, 1.0]
cnt1: [4.0, 6.5, -0.080, 0.70, -0.52, 0.96891, 0.0, 0.2474, 0.0, 1.0]
cnt2: [8.5, 9.9, -0.080, 1.25, -0.25, 1.0, 0.0, 0.0, 0.0, 1.0]
- effcnt_rh:
- effcnt_lh:
+ eefcnt_rh:
+ eefcnt_lh:
planner_variables:
@@ -71,10 +71,10 @@ planner_variables:
friction_coeff: 1.0
friction_cone: LinearCone # Types: LinearCone, SocCone #
torque_range: [-200.0, 200.0]
- eff_offset_rf: [ 0.00, 0.00, 0.00]
- eff_offset_lf: [ 0.00, 0.00, 0.00]
- eff_offset_rh: [ 0.20, 0.00, 0.48]
- eff_offset_lh: [-0.20, 0.00, 0.48]
+ eef_offset_rf: [ 0.00, 0.00, 0.00]
+ eef_offset_lf: [ 0.00, 0.00, 0.00]
+ eef_offset_rh: [ 0.20, 0.00, 0.48]
+ eef_offset_lh: [-0.20, 0.00, 0.48]
cop_range_rf: [-0.03, 0.03, -0.08, 0.08]
cop_range_lf: [-0.03, 0.03, -0.08, 0.08]
cop_range_rh: [-0.03, 0.03, -0.03, 0.03]
diff --git a/Config/Draco/CENTROID_PLANNER_RESULT.yaml b/Config/Draco/CENTROID_PLANNER_RESULT.yaml
new file mode 100644
index 00000000..5f52ea29
--- /dev/null
+++ b/Config/Draco/CENTROID_PLANNER_RESULT.yaml
@@ -0,0 +1,2894 @@
+dynopt_params:
+ time_step: 0.1
+ end_com:
+ - 0.2
+ - 1.2
+ - 0.4
+ robot_mass: 60
+ n_act_eefs: 2
+ ini_com:
+ - 0
+ - 0.15
+ - -0.2
+ time_horizon: 9.5
+ com_motion:
+ -
+ - -0.002072137346209411
+ - -0.005823053815307695
+ - -0.01087060655538419
+ - -0.01684129666320853
+ - -0.02336778169294801
+ - -0.03008645013520949
+ - -0.03665377307710405
+ - -0.04273825665162349
+ - -0.04810013345118826
+ - -0.0521780125614868
+ - -0.05455717875591393
+ - -0.05481157701393213
+ - -0.0525009894556628
+ - -0.04716809573312016
+ - -0.03833542383070455
+ - -0.02530119321796137
+ - -0.005638305370044762
+ - 0.024286471524423
+ - 0.06982363058386691
+ - 0.1204771485710522
+ - 0.1746046288610813
+ - 0.2306122602610615
+ - 0.2869386899976171
+ - 0.3420623175781078
+ - 0.3837773574206112
+ - 0.4162801586032435
+ - 0.4431861263160164
+ - 0.4647181820931665
+ - 0.4810626405066802
+ - 0.4923711605756122
+ - 0.4988067959879737
+ - 0.5007141454313603
+ - 0.4978360507493687
+ - 0.4901352537251592
+ - 0.477542176420279
+ - 0.4599544401560267
+ - 0.4367894983793248
+ - 0.4038974461874023
+ - 0.3554063124906342
+ - 0.3018276413329194
+ - 0.2449477523513359
+ - 0.1865334116677805
+ - 0.1283435386102741
+ - 0.07213859143688976
+ - 0.02985398932393682
+ - -0.003200618915707735
+ - -0.03002400377086518
+ - -0.05099484910323928
+ - -0.06645283236067574
+ - -0.07670090116209341
+ - -0.08236034723612436
+ - -0.0837896548759932
+ - -0.08066120124917332
+ - -0.0729987240607442
+ - -0.06079817509264291
+ - -0.04402526485749055
+ - -0.02250707020864291
+ - 0.007487334742573514
+ - 0.05118589912044116
+ - 0.09942551619293408
+ - 0.1506343587317492
+ - 0.2032603777822087
+ - 0.2557611421076795
+ - 0.3066092512845421
+ - 0.3450083028925262
+ - 0.3752785175861678
+ - 0.4003495972893798
+ - 0.4207062656785322
+ - 0.4368018147190525
+ - 0.4490611274642645
+ - 0.4578835102494554
+ - 0.4637766201634515
+ - 0.4673624634476533
+ - 0.4689577358163007
+ - 0.468579626888991
+ - 0.4662085993807885
+ - 0.4619580487459217
+ - 0.4559288466785146
+ - 0.4482100072237996
+ - 0.4388792811471647
+ - 0.4280036750430837
+ - 0.4156397239992657
+ - 0.4005091732965314
+ - 0.3799955854293195
+ - 0.3587623968062112
+ - 0.3375728272591142
+ - 0.3170411319369195
+ - 0.2976539710430791
+ - 0.2798007756788898
+ - 0.2638514997979283
+ - 0.2501745301375133
+ - 0.2391442018303708
+ - 0.2311416713414603
+ - 0.2265872377219914
+ - 0.2259304882456665
+ -
+ - 0.1503118976791344
+ - 0.1509717398550951
+ - 0.1520158068652605
+ - 0.153480635159371
+ - 0.1554030416757557
+ - 0.1578201484011202
+ - 0.1607693900272228
+ - 0.1642885319057074
+ - 0.1684155335566519
+ - 0.173189272922202
+ - 0.1786487734164103
+ - 0.1848335166649334
+ - 0.1917834673729968
+ - 0.199539100251354
+ - 0.2081414267835487
+ - 0.2176320259248812
+ - 0.22805309278509
+ - 0.2397337630943978
+ - 0.2541153979365508
+ - 0.2695201524404122
+ - 0.2857269222351597
+ - 0.302511107944923
+ - 0.3196437895599464
+ - 0.3368804724006202
+ - 0.3520390482923696
+ - 0.3666240904005359
+ - 0.3808119246982676
+ - 0.3946433919040382
+ - 0.40815908748268
+ - 0.4213993949240268
+ - 0.434404512137878
+ - 0.447214481751239
+ - 0.4598692104468045
+ - 0.4724085020262799
+ - 0.484872082872974
+ - 0.4972996297851202
+ - 0.5097307980173518
+ - 0.5222052570681072
+ - 0.5352514494781243
+ - 0.5484235443739462
+ - 0.561689089273238
+ - 0.575017512454667
+ - 0.5883787909877621
+ - 0.6017429275004317
+ - 0.6144411455852976
+ - 0.627014616044602
+ - 0.6394950914724504
+ - 0.6519142587001695
+ - 0.664303764506007
+ - 0.676695236418424
+ - 0.6891203013346895
+ - 0.7016106089145314
+ - 0.7141978592208454
+ - 0.7269138122501096
+ - 0.7397903135051068
+ - 0.7528593151649641
+ - 0.7661528992466049
+ - 0.7797033645515964
+ - 0.7948806067073506
+ - 0.8103805466030196
+ - 0.8260446191909562
+ - 0.841714370450287
+ - 0.8572313266188477
+ - 0.8724350426003866
+ - 0.885772844407412
+ - 0.8984204926273469
+ - 0.9104397444578542
+ - 0.9218919333547723
+ - 0.9328380161159494
+ - 0.9433386119220321
+ - 0.953454042524287
+ - 0.963244374406429
+ - 0.9727694582402757
+ - 0.9820889627891801
+ - 0.991262416506576
+ - 1.000349251707134
+ - 1.009408843805149
+ - 1.018500549511174
+ - 1.027683746554018
+ - 1.037017873490172
+ - 1.046562469704321
+ - 1.056377216474389
+ - 1.067103103252729
+ - 1.08012259081445
+ - 1.093583574546984
+ - 1.107178475857333
+ - 1.120592457064052
+ - 1.133504046580692
+ - 1.145587140930875
+ - 1.156520951356442
+ - 1.165991250086902
+ - 1.173681338523345
+ - 1.179274143840139
+ - 1.182455834279903
+ - 1.182912098648596
+ -
+ - -0.2000409062075584
+ - -0.2001183179414399
+ - -0.200227839229224
+ - -0.2003650982678487
+ - -0.2005257452320889
+ - -0.2007054520706695
+ - -0.2009001349832316
+ - -0.201106175783817
+ - -0.2013227974245364
+ - -0.201541824930291
+ - -0.2017601995036647
+ - -0.2019748782489338
+ - -0.2021828374234962
+ - -0.2023810749044072
+ - -0.202566612292049
+ - -0.2027447951999488
+ - -0.2030468947480387
+ - -0.2038938260052155
+ - -0.20649733025598
+ - -0.2089630113744676
+ - -0.2109723033143737
+ - -0.2122052288839222
+ - -0.2123486857256545
+ - -0.2111113570779221
+ - -0.209919127684416
+ - -0.2078414826273235
+ - -0.2046087999806671
+ - -0.2003087336683859
+ - -0.1950282655840894
+ - -0.1888535905656186
+ - -0.1818679475735731
+ - -0.1741446777367641
+ - -0.1657816855987761
+ - -0.1568648684695208
+ - -0.1474794839006801
+ - -0.1377102054954451
+ - -0.127667832196009
+ - -0.1178944028199058
+ - -0.1098350058861905
+ - -0.1016530818880407
+ - -0.09311468827987898
+ - -0.08398684419526474
+ - -0.07403619260932175
+ - -0.06302881021193002
+ - -0.05245571453541627
+ - -0.04134318358442289
+ - -0.02951091532502872
+ - -0.01703940188710262
+ - -0.004008735817444574
+ - 0.009501328086925059
+ - 0.02342770405511713
+ - 0.0376949195851092
+ - 0.05220061054004036
+ - 0.06685845525488469
+ - 0.08158229769963943
+ - 0.09628597921851643
+ - 0.1108781529650813
+ - 0.1249337855425522
+ - 0.1373091489509918
+ - 0.1493657087944807
+ - 0.1612440277218002
+ - 0.1730845303742171
+ - 0.1850275922819382
+ - 0.1972143117332901
+ - 0.2082862423299671
+ - 0.2191382200096521
+ - 0.2299671690803903
+ - 0.2407436532110606
+ - 0.2514382066875963
+ - 0.2620213182472858
+ - 0.2724634152586272
+ - 0.282738697077788
+ - 0.2928225000768055
+ - 0.3026831257588578
+ - 0.3122843811670892
+ - 0.3215894482459618
+ - 0.330564376832177
+ - 0.33917507494036
+ - 0.3473872990547826
+ - 0.3551666433735714
+ - 0.3624785279013795
+ - 0.3692881637041341
+ - 0.3755041339844409
+ - 0.3808000135686416
+ - 0.3853682935817812
+ - 0.3892531056832959
+ - 0.39250931812968
+ - 0.3951953455944245
+ - 0.3973681750301481
+ - 0.3990772614568206
+ - 0.4003684321432133
+ - 0.4012837926437273
+ - 0.4018647006482604
+ - 0.402150893941257
+ - 0.4021829642715027
+ lin_mom:
+ -
+ - -1.243266546178571
+ - -2.250533360468878
+ - -3.028514748867568
+ - -3.582397083516002
+ - -3.91587429963367
+ - -4.031185190230278
+ - -3.940381492998821
+ - -3.650690664543354
+ - -3.217133786036763
+ - -2.446735936554923
+ - -1.427504550234302
+ - -0.1526432438424734
+ - 1.386348578965696
+ - 3.19973254050751
+ - 5.299599748904782
+ - 7.820535504811279
+ - 11.7977319287849
+ - 17.95486552947914
+ - 27.32229431885231
+ - 30.39210784699192
+ - 32.47648466583953
+ - 33.60457576942486
+ - 33.79585995464033
+ - 33.07418349952534
+ - 25.02903610748428
+ - 19.5016851877283
+ - 16.14358153633557
+ - 12.91923210057812
+ - 9.806673847926678
+ - 6.785111359927653
+ - 3.86138152313042
+ - 1.144408956325135
+ - -1.726859468681264
+ - -4.620478155065142
+ - -7.555845513254053
+ - -10.55264046077929
+ - -13.89896371625802
+ - -19.73523328283614
+ - -29.09468410202787
+ - -32.14720935828014
+ - -34.12793530747027
+ - -35.04860112600429
+ - -34.91392014708931
+ - -33.72296558343627
+ - -25.37075786083284
+ - -19.83276476763286
+ - -16.09403102335646
+ - -12.58250614946166
+ - -9.274788482097289
+ - -6.148839739016951
+ - -3.395666852945018
+ - -0.8575818102906101
+ - 1.87707823703396
+ - 4.597482068615746
+ - 7.320324986788794
+ - 10.0637420084805
+ - 12.91091326062423
+ - 17.9966421422072
+ - 26.21913878625955
+ - 28.94377224669038
+ - 30.72530562131755
+ - 31.5756103299543
+ - 31.50045809409484
+ - 30.50886911400968
+ - 23.03943621200121
+ - 18.16213069845702
+ - 15.04264766999857
+ - 12.21399985650547
+ - 9.657328048994046
+ - 7.355586341247569
+ - 5.293428699556222
+ - 3.535865851807427
+ - 2.151506576291795
+ - 0.9571634704023737
+ - -0.2268658812041496
+ - -1.422616865896329
+ - -2.550329535760388
+ - -3.617519910257446
+ - -4.631302109840113
+ - -5.598434018889467
+ - -6.525362136099155
+ - -7.418369446022262
+ - -9.078330411552322
+ - -12.30815426601517
+ - -12.73991725065718
+ - -12.71374424287196
+ - -12.31901931298748
+ - -11.63229895226032
+ - -10.71191948067036
+ - -9.569566261264024
+ - -8.20617954551377
+ - -6.618190753735573
+ - -4.801507240277746
+ - -2.732643205194967
+ - -0.3940298257375142
+ -
+ - 0.1871376560076453
+ - 0.395904355102447
+ - 0.6264392502342271
+ - 0.8788960104507054
+ - 1.153442931440864
+ - 1.450263045602398
+ - 1.769543980340089
+ - 2.111484165028367
+ - 2.476200107288045
+ - 2.864242817942566
+ - 3.275699806341769
+ - 3.710845498764576
+ - 4.169970033045286
+ - 4.653379418769469
+ - 5.161395729319243
+ - 5.69435946539126
+ - 6.252640360125195
+ - 7.008402932921663
+ - 8.628981853528767
+ - 9.242853480320926
+ - 9.724062758491385
+ - 10.07051244384079
+ - 10.27961005233658
+ - 10.34201077767767
+ - 9.095146814620133
+ - 8.751026506568842
+ - 8.512700763288532
+ - 8.298880066497091
+ - 8.109416860620112
+ - 7.944183840024608
+ - 7.8030696217433
+ - 7.685981022108225
+ - 7.59283646878669
+ - 7.523574232424735
+ - 7.478147865297703
+ - 7.456527625459468
+ - 7.458700607112499
+ - 7.48467540909107
+ - 7.827716077807981
+ - 7.903258406345381
+ - 7.959328599854602
+ - 7.997055533701383
+ - 8.01676866313443
+ - 8.018483294451348
+ - 7.618931345084675
+ - 7.544082637891504
+ - 7.48828495171198
+ - 7.451499724196064
+ - 7.433702685218335
+ - 7.434882231512252
+ - 7.455037964004464
+ - 7.494183530681872
+ - 7.552349169943288
+ - 7.629570843004436
+ - 7.725899858561101
+ - 7.841400234668533
+ - 7.976149902921237
+ - 8.130279021216285
+ - 9.106346609434356
+ - 9.299965339574042
+ - 9.398445125664415
+ - 9.401852387925977
+ - 9.310175345863902
+ - 9.122231129370254
+ - 8.002682098588668
+ - 7.588591129899005
+ - 7.211551687248041
+ - 6.871313526957173
+ - 6.567649617089046
+ - 6.300357295131935
+ - 6.069258072964641
+ - 5.87419877624391
+ - 5.715049910642487
+ - 5.591702327167949
+ - 5.504071837989054
+ - 5.452100759520517
+ - 5.435754952697088
+ - 5.455023198279255
+ - 5.509918113027092
+ - 5.600476204054488
+ - 5.726757989588896
+ - 5.888848659058701
+ - 6.435533375543329
+ - 7.811693347777891
+ - 8.076588542699808
+ - 8.156939249542635
+ - 8.048387420561962
+ - 7.746952728439789
+ - 7.24985609103467
+ - 6.560286451274077
+ - 5.68218088679618
+ - 4.614056096752077
+ - 3.355687774874196
+ - 1.909020850060115
+ - 0.2737680515429857
+ -
+ - -0.02455368804901903
+ - -0.0464564357199192
+ - -0.06572155127599737
+ - -0.08236351135689431
+ - -0.09639548142003283
+ - -0.1078305002549913
+ - -0.1168151072770375
+ - -0.1236290412746042
+ - -0.1299769411625442
+ - -0.1314203554159306
+ - -0.1310278067707826
+ - -0.1288094531619294
+ - -0.1247768204719533
+ - -0.1189428949545765
+ - -0.1113219265231043
+ - -0.1069083460465061
+ - -0.1812578136717487
+ - -0.5081565272568281
+ - -1.562100386958639
+ - -1.479406284560846
+ - -1.205572308433138
+ - -0.7397523692695491
+ - -0.08607124452298623
+ - 0.742400444646853
+ - 0.7153462202779971
+ - 1.246595155052441
+ - 1.939617471596324
+ - 2.580046391613538
+ - 3.168286283557454
+ - 3.704809310828319
+ - 4.19138902674713
+ - 4.63396385417329
+ - 5.017796436958953
+ - 5.350090569007927
+ - 5.631230033351434
+ - 5.861565340045471
+ - 6.025421293148343
+ - 5.864054117077331
+ - 4.835634473443567
+ - 4.909150815710832
+ - 5.123032207982705
+ - 5.476701960242978
+ - 5.970386433395623
+ - 6.604425688978202
+ - 6.343848962857516
+ - 6.667510426238291
+ - 7.09935295321511
+ - 7.482900929968078
+ - 7.818393587825573
+ - 8.106033478768651
+ - 8.355822038073297
+ - 8.560326360060845
+ - 8.703412085500936
+ - 8.794706411769642
+ - 8.834306253832722
+ - 8.822210802650266
+ - 8.755307104778058
+ - 8.433382796637346
+ - 7.425221083920009
+ - 7.233937955031378
+ - 7.1269949915893
+ - 7.104306491235117
+ - 7.165842831588932
+ - 7.312037883274385
+ - 6.643165068132264
+ - 6.511193468139909
+ - 6.497376154686159
+ - 6.465896677981694
+ - 6.416737680487108
+ - 6.349871823250544
+ - 6.265262295115172
+ - 6.165172320066948
+ - 6.050284427710683
+ - 5.916377505447612
+ - 5.760754783854226
+ - 5.583041107737317
+ - 5.384956949892959
+ - 5.166417588283143
+ - 4.927332123037234
+ - 4.667603186124943
+ - 4.387126260429761
+ - 4.085775977725913
+ - 3.72957561535499
+ - 3.177521485318961
+ - 2.740962454924068
+ - 2.330882681560455
+ - 1.953723519021038
+ - 1.611612735926884
+ - 1.303693922289728
+ - 1.0254480267604
+ - 0.7746984552704761
+ - 0.5492122084475702
+ - 0.348540590648599
+ - 0.1717115090281607
+ - 0.01923757503412861
+ ang_mom:
+ -
+ - 0.0005877395613670278
+ - 0.001206100889409799
+ - 0.001885802382426903
+ - 0.002660384953196525
+ - 0.003567981928132734
+ - 0.004653360894795121
+ - 0.00597265369814818
+ - 0.007595738124725415
+ - 0.00962741837880457
+ - 0.01211268248557367
+ - 0.01520975658238431
+ - 0.01907566316266472
+ - 0.02390605547089669
+ - 0.0299450184388942
+ - 0.03749730581140723
+ - 0.04694392615079689
+ - 0.05873582774954003
+ - 0.0301923488185085
+ - -0.2075355196577065
+ - -0.1987751082670521
+ - -0.1973184225070628
+ - -0.2025342503607308
+ - -0.2139840766954024
+ - -0.230278794230762
+ - 0.1171868242460685
+ - 0.1223549013124033
+ - 0.1021819706466227
+ - 0.08618139476887843
+ - 0.07353865845664766
+ - 0.06361906680521345
+ - 0.05592427081037014
+ - 0.05006693725444424
+ - 0.045755267021569
+ - 0.04277136184828527
+ - 0.04096556406807948
+ - 0.04024750832792015
+ - 0.0405817351054649
+ - 0.04198629155193863
+ - -0.03267994384186741
+ - -0.0324704753810412
+ - -0.03395083068223137
+ - -0.03736386837649697
+ - -0.04297334085056193
+ - -0.05105326509052943
+ - 0.08008762017130278
+ - 0.07157336777558862
+ - 0.06504154968036117
+ - 0.06019597529212684
+ - 0.05679310844230226
+ - 0.05466192758705043
+ - 0.05369371163223333
+ - 0.05384007679227053
+ - 0.05511222798162488
+ - 0.05757253448911136
+ - 0.06134501579906675
+ - 0.06661977872348353
+ - 0.07366421321346253
+ - 0.08285112146426789
+ - -0.1193724759322337
+ - -0.1134105561955541
+ - -0.1128043088989603
+ - -0.1175241068688435
+ - -0.1278203706117282
+ - -0.1439770741515276
+ - 0.1484512050805458
+ - 0.1301711264232813
+ - 0.1157985005257006
+ - 0.1047390293980015
+ - 0.0964377925929305
+ - 0.09047826442450665
+ - 0.08656145846202738
+ - 0.0844916197755662
+ - 0.08416585065135296
+ - 0.08556552690420609
+ - 0.0887578063945778
+ - 0.09390231933112507
+ - 0.1012568291461608
+ - 0.1111889488191291
+ - 0.124195212296945
+ - 0.140925893314965
+ - 0.1622175037455687
+ - 0.1891343295456767
+ - 0.1264627901813241
+ - -0.1512380809563752
+ - -0.1282151286093813
+ - -0.1107769568268067
+ - -0.09699729464028017
+ - -0.08523822611215026
+ - -0.07427894041388727
+ - -0.06426386185889088
+ - -0.05566482949367835
+ - -0.04778810967539426
+ - -0.04036930681995132
+ - -0.03370803600740757
+ - -0.02763762818057813
+ -
+ - -0.003947584272206761
+ - -0.007927325786602157
+ - -0.0119922167288915
+ - -0.0161986168499868
+ - -0.0206057402096298
+ - -0.02526812238105158
+ - -0.03089234947752706
+ - -0.03788827602185352
+ - -0.04938473232854568
+ - -0.05593306814411578
+ - -0.06316427545182383
+ - -0.07117000132345543
+ - -0.08005113736468407
+ - -0.0899197852050352
+ - -0.1009003413788878
+ - -0.1019832831538561
+ - 0.007097508580255971
+ - 0.2973510469616309
+ - 0.8870320342432998
+ - 0.8791400481332747
+ - 0.876241061430715
+ - 0.8791937335951889
+ - 0.8885897311072473
+ - 0.9057995270853851
+ - 0.1581995001952335
+ - -0.1189268694098115
+ - -0.1023341202284326
+ - -0.08700878514131077
+ - -0.07265545076655656
+ - -0.05909490087039097
+ - -0.04409195304170063
+ - -0.01035748727778153
+ - 0.002005295872448902
+ - 0.01438354046071641
+ - 0.02693138298733732
+ - 0.03980412304919351
+ - 0.026947230181358
+ - -0.2373060728410763
+ - -0.8570219546704166
+ - -0.8494020536423985
+ - -0.8473160625470708
+ - -0.8505276761979941
+ - -0.8589851929818728
+ - -0.8727441377502103
+ - -0.003299669156999085
+ - 0.2533560706567276
+ - 0.2359219850116742
+ - 0.2200699401925033
+ - 0.2055999349109709
+ - 0.1923297887785961
+ - 0.1524748744593291
+ - 0.1046937021434415
+ - 0.09330980112405249
+ - 0.08222465013931336
+ - 0.07116123587852231
+ - 0.05998134634822586
+ - 0.05512795349584311
+ - 0.2821967196622324
+ - 0.8254590985317811
+ - 0.819471708843631
+ - 0.8180833285591618
+ - 0.8212704317895438
+ - 0.829064654802635
+ - 0.8422338103196205
+ - 0.0509104398134036
+ - -0.1898165976834615
+ - -0.1745085636074704
+ - -0.1608326779540927
+ - -0.1486113997123367
+ - -0.1376921426057811
+ - -0.1279408491387143
+ - -0.1097471870346736
+ - -0.0729913863878119
+ - -0.03747948122512542
+ - -0.01845994513590348
+ - -0.01164542135232283
+ - -0.004879059425085403
+ - 0.001924692594341709
+ - 0.008851566116126469
+ - 0.01598866641609364
+ - 0.02342562189522017
+ - 0.03126780295640936
+ - -0.03750752125844164
+ - -0.2681124817612716
+ - -0.2443497401430532
+ - -0.2217274163867824
+ - -0.2019346963813629
+ - -0.1860147772857967
+ - -0.1745702952172259
+ - -0.1664203508409052
+ - -0.1604706860775655
+ - -0.1560688081857773
+ - -0.1534742601875383
+ - -0.151363445705468
+ - -0.149386107331102
+ -
+ - -0.0002839992582023988
+ - -0.0005822150655558009
+ - -0.0009096147269750041
+ - -0.001282622444444622
+ - -0.001719937540288711
+ - -0.002242981168885504
+ - -0.002873836745952842
+ - -0.003638457784526679
+ - -0.004538703841432375
+ - -0.004619661211435012
+ - -0.004731653015082288
+ - -0.004883121121345006
+ - -0.005085827554980001
+ - -0.005356038765380959
+ - -0.005716150265685884
+ - -0.00630958932048916
+ - -0.00826662967547136
+ - -0.009269349448006062
+ - 0.01066079344348446
+ - 0.006317171441360056
+ - 0.001956368972040358
+ - -0.002193972925950381
+ - -0.006235232540218188
+ - -0.01094150504261357
+ - 0.03139191836308142
+ - 0.03099253731439387
+ - 0.02966678991332605
+ - 0.02864186857166248
+ - 0.02784080168133546
+ - 0.02721582525285212
+ - 0.02673889639630846
+ - 0.02642152620042558
+ - 0.02614766270851953
+ - 0.02595510378500499
+ - 0.02583632770397333
+ - 0.02578771348114155
+ - 0.02581194356959249
+ - 0.02608459854306531
+ - 0.0155500295581385
+ - 0.01699104447831737
+ - 0.01807195230664625
+ - 0.01906754756508901
+ - 0.0201894021799965
+ - 0.02149080802544251
+ - 0.004683176964189257
+ - 0.005805651311608068
+ - 0.006224969710731118
+ - 0.006511755205032203
+ - 0.006691950655171025
+ - 0.00678367237576601
+ - 0.006804736516698007
+ - 0.006774601634138737
+ - 0.006655023298331291
+ - 0.006465719358954064
+ - 0.006199690713888433
+ - 0.005845432358255388
+ - 0.005364341793237075
+ - 0.003849858905957528
+ - 0.02940273473900823
+ - 0.02607279930711884
+ - 0.0230256460019151
+ - 0.0201084887324973
+ - 0.0171692524783103
+ - 0.01438381652606013
+ - 0.03474212496258985
+ - 0.03155245058273008
+ - 0.03074733983855081
+ - 0.03016001970630593
+ - 0.02973517564619218
+ - 0.02943244817132863
+ - 0.02922244104912063
+ - 0.02911552129970754
+ - 0.02910642769412637
+ - 0.02911815297941288
+ - 0.02912990425575818
+ - 0.02916860284542758
+ - 0.02924640608393815
+ - 0.02936571304706522
+ - 0.02953083123145108
+ - 0.02974817821883744
+ - 0.03002664308478999
+ - 0.03037777593617241
+ - 0.02934976369624307
+ - 0.02032621951774979
+ - 0.02338992790439819
+ - 0.02567484996085313
+ - 0.02725038005617244
+ - 0.02818864415494971
+ - 0.02855186910250305
+ - 0.02861234340823716
+ - 0.02852589861476306
+ - 0.02835253707514187
+ - 0.02808743836226786
+ - 0.02786745632623031
+ - 0.02769593337279368
+ com_motion_ref:
+ -
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ -
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ -
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ lin_mom_ref:
+ -
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ -
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ -
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ ang_mom_ref:
+ -
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ -
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ -
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ time_vec:
+ -
+ - 0.1
+ - 0.2
+ - 0.3
+ - 0.4
+ - 0.5
+ - 0.6
+ - 0.7
+ - 0.7999999999999999
+ - 0.8999999999999999
+ - 0.9999999999999999
+ - 1.1
+ - 1.2
+ - 1.3
+ - 1.4
+ - 1.5
+ - 1.6
+ - 1.7
+ - 1.8
+ - 1.900000000000001
+ - 2
+ - 2.100000000000001
+ - 2.200000000000001
+ - 2.300000000000001
+ - 2.400000000000001
+ - 2.500000000000001
+ - 2.600000000000001
+ - 2.700000000000001
+ - 2.800000000000001
+ - 2.900000000000001
+ - 3.000000000000001
+ - 3.100000000000001
+ - 3.200000000000002
+ - 3.300000000000002
+ - 3.400000000000002
+ - 3.500000000000002
+ - 3.600000000000002
+ - 3.700000000000002
+ - 3.800000000000002
+ - 3.900000000000002
+ - 4.000000000000002
+ - 4.100000000000001
+ - 4.200000000000001
+ - 4.300000000000001
+ - 4.4
+ - 4.5
+ - 4.6
+ - 4.699999999999999
+ - 4.799999999999999
+ - 4.899999999999999
+ - 4.999999999999998
+ - 5.099999999999998
+ - 5.199999999999998
+ - 5.299999999999997
+ - 5.399999999999997
+ - 5.499999999999996
+ - 5.599999999999996
+ - 5.699999999999996
+ - 5.799999999999995
+ - 5.899999999999995
+ - 5.999999999999995
+ - 6.099999999999994
+ - 6.199999999999994
+ - 6.299999999999994
+ - 6.399999999999993
+ - 6.499999999999993
+ - 6.599999999999993
+ - 6.699999999999992
+ - 6.799999999999992
+ - 6.899999999999991
+ - 6.999999999999991
+ - 7.099999999999991
+ - 7.19999999999999
+ - 7.29999999999999
+ - 7.39999999999999
+ - 7.499999999999989
+ - 7.599999999999989
+ - 7.699999999999989
+ - 7.799999999999988
+ - 7.899999999999988
+ - 7.999999999999988
+ - 8.099999999999987
+ - 8.199999999999987
+ - 8.299999999999986
+ - 8.399999999999986
+ - 8.499999999999986
+ - 8.599999999999985
+ - 8.699999999999985
+ - 8.799999999999985
+ - 8.899999999999984
+ - 8.999999999999984
+ - 9.099999999999984
+ - 9.199999999999983
+ - 9.299999999999983
+ - 9.399999999999983
+ - 9.499999999999982
+ eef_frc_0:
+ -
+ - -0.01036030810599501
+ - -0.008359743410107824
+ - -0.006373089244830456
+ - -0.0043528787378734
+ - -0.002349691352080868
+ - -0.0004335915037213627
+ - -0.0007655141712262337
+ - -0.0003230291179306385
+ - -0.002025409126694268
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0.08488965588576566
+ - 0.06196474771471102
+ - 0.02437269160407983
+ - -0.02665346202270012
+ - -0.07432746730257354
+ - -0.1366827592736843
+ - -0.09390674098813757
+ - -0.05705238797025221
+ - -0.0547799768441186
+ - -0.05288070444891201
+ - -0.05133473516733184
+ - -0.04967260963705301
+ - -0.04615991309775049
+ - -0.04878132018141364
+ - -0.0491610381561507
+ - -0.04987032563851338
+ - -0.05091394744045402
+ - -0.05685224474091562
+ - -0.09915510490327185
+ - -0.1590120750117754
+ - 0.02489007816065666
+ - -0.006151714843207175
+ - -0.01877452769629371
+ - -0.04709150431161392
+ - -0.08381361585896835
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0.08705876263955448
+ - 0.03319194417755927
+ - 0.006227074168721815
+ - -0.03235157302854431
+ - -0.08564645237166765
+ - -0.1269016789338301
+ - -0.08286281761513849
+ - -0.05299835201078763
+ - -0.04805721724072508
+ - -0.04343649009752699
+ - -0.03910536381069329
+ - -0.03503495860528721
+ - -0.02986005551273174
+ - -0.02351952524256088
+ - -0.02029125184314188
+ - -0.02011602697171893
+ - -0.02031517185910545
+ - -0.0191592367904591
+ - -0.01813099515382532
+ - -0.01722361867727877
+ - -0.01643105505438331
+ - -0.01574801396244895
+ - -0.01517171704783973
+ - -0.0282018497430375
+ - -0.05487298245743477
+ - 0.01288440233820284
+ - 0.01315663950526589
+ - 0.01329162488878083
+ - 0.01344304784725766
+ - 0.01322597261977312
+ - 0.0118422798277926
+ - 0.01222234107401826
+ - 0.01490203884646537
+ - 0.01692199008759706
+ - 0.01992265461116436
+ - 0.02223365001885471
+ -
+ - 0.001566001521549666
+ - 0.001747830483602139
+ - 0.001929032674507703
+ - 0.002110098301373992
+ - 0.00229127952511137
+ - 0.002470908174506987
+ - 0.002433638392750467
+ - 0.0023504674226535
+ - 0.002366963734622732
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0.03042057153819271
+ - 0.02265262768594105
+ - 0.009025303924860201
+ - -0.009288242247828477
+ - -0.0166173821718996
+ - -0.02118355370456092
+ - -0.00584641991366018
+ - -0.004049027072373544
+ - -0.003632699511690857
+ - -0.003218878774576898
+ - -0.002807220872890248
+ - -0.002397455300608843
+ - -0.001989272864565111
+ - -0.001582476301228621
+ - -0.001176728487602198
+ - -0.0007717698013372999
+ - -0.0003673164261567849
+ - 3.691765787828915e-05
+ - 0.0004412977108422692
+ - 0.005828077740207961
+ - -0.01868264212022766
+ - -0.003724136326072975
+ - 0.004348626074412415
+ - 0.01463392504993311
+ - 0.01561906478351316
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0.03198904138141066
+ - 0.01004442885106274
+ - -0.0005530894876111244
+ - -0.01593141351798785
+ - -0.01980050846164818
+ - -0.01902054150375313
+ - -0.007035183875859479
+ - -0.006405698837153932
+ - -0.00578046476528901
+ - -0.005159087797396229
+ - -0.004541153941203484
+ - -0.003926252495506597
+ - -0.003313953396291085
+ - -0.002703854334471816
+ - -0.002095609654933472
+ - -0.001488795281799523
+ - -0.0008829609230366625
+ - -0.0002777065707798464
+ - 0.000327357169043757
+ - 0.0009326352545634884
+ - 0.001538533564006945
+ - 0.002145460015387081
+ - 0.002753833642547321
+ - 0.009287881901046447
+ - 0.02338022479426152
+ - -0.003710136422739866
+ - -0.00430601061346729
+ - -0.005022664857769595
+ - -0.005924641305711978
+ - -0.006807273476202041
+ - -0.007022876170230417
+ - -0.007907078620877472
+ - -0.009819117638752329
+ - -0.01138117772781719
+ - -0.01333750112563168
+ - -0.01489544869615435
+ -
+ - 0.5024413888527977
+ - 0.5019947523259372
+ - 0.501577504391614
+ - 0.5012493070080479
+ - 0.5010601430305047
+ - 0.5007288795258424
+ - 0.455339871491745
+ - 0.4060075834123582
+ - 0.36508132856451
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0.3278618653192862
+ - 0.3965698311221423
+ - 0.4799978697971063
+ - 0.5832461821135914
+ - 0.6853583376630165
+ - 0.9995403634952085
+ - 1.009025636104614
+ - 1.011774079447013
+ - 1.010880545665518
+ - 1.009993881926713
+ - 1.009115239989317
+ - 1.008266729811549
+ - 1.007519110230426
+ - 1.006521110787533
+ - 1.005645500039352
+ - 1.004776409534932
+ - 1.003913274009591
+ - 1.002783825323211
+ - 0.9972584578662277
+ - 0.9825276989943434
+ - 0.6431045454826463
+ - 0.544527177911851
+ - 0.4696107422060216
+ - 0.4112669987179129
+ - 0.3303993022365793
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0.3683143938430207
+ - 0.4551218143796516
+ - 0.5014811443109315
+ - 0.5661590281757012
+ - 0.6755194756227331
+ - 0.9886362077776307
+ - 0.9977578729251931
+ - 0.9997652511617811
+ - 0.9994651804202952
+ - 0.9991648147927122
+ - 0.9988639846914097
+ - 0.9985625291853875
+ - 0.9982995246481281
+ - 0.9980481159342065
+ - 0.9977249927626604
+ - 0.9973560529167753
+ - 0.9969807393883633
+ - 0.9966346557778718
+ - 0.9962871327818147
+ - 0.9959380654450989
+ - 0.9955873438778576
+ - 0.995234846621621
+ - 0.9948802194288967
+ - 0.9939483458326076
+ - 0.9906208948231979
+ - 0.7730754053365947
+ - 0.7244167794334078
+ - 0.6788271508123409
+ - 0.6369179736556613
+ - 0.5990450219462531
+ - 0.5648970230123195
+ - 0.5348790203289021
+ - 0.509609768382396
+ - 0.4899698330992609
+ - 0.4770348216201475
+ - 0.4714343683967342
+ eef_trq_0:
+ -
+ - 4.915746722682453e-07
+ - 5.070050348324143e-07
+ - 5.564290190464287e-07
+ - 6.497545803236095e-07
+ - 8.044687509494479e-07
+ - 1.039575035985877e-06
+ - -2.372981323399444e-06
+ - -9.025845255356346e-06
+ - -3.606343397528267e-05
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 3.309130332563815e-08
+ - -2.283521980967748e-08
+ - -0.0001160063074395361
+ - 2.591923483162551e-06
+ - 0.00153140338939556
+ - 0.11305943966152
+ - 0.007865370260942754
+ - -0.002814767715224631
+ - -0.002356413962780838
+ - -0.002047995359516584
+ - -0.001752600791019931
+ - -0.001481528360030458
+ - -0.0012853104997651
+ - -0.0009361932065033634
+ - -0.0006789464996810879
+ - -0.0004303003529162945
+ - -0.0001874666224892409
+ - 5.314042452810981e-05
+ - 0.0001736628326630806
+ - -0.02226380304704293
+ - -0.0005451972939988186
+ - -0.0001151873422945741
+ - -9.574993590472156e-07
+ - 3.396710005817821e-08
+ - 7.156152536658752e-07
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - -6.778604948891323e-07
+ - -6.115122509805054e-07
+ - -2.576928740382939e-07
+ - -6.377734655379753e-06
+ - -0.0003765566658703653
+ - -0.01953478666901068
+ - 0.0002583299272821156
+ - -0.001412880820208876
+ - -0.001346436005745807
+ - -0.00127069420304417
+ - -0.001190553307742591
+ - -0.001109256009620286
+ - -0.001054046232188619
+ - -0.001009933790404573
+ - -0.0009132943962803216
+ - -0.0008102619830942467
+ - -0.000721647583178061
+ - -0.0006546476583156057
+ - -0.0005920779512464389
+ - -0.0005332378872074611
+ - -0.0004774484425066978
+ - -0.0004240903512871452
+ - -0.0003763596246535475
+ - 0.001075359523252144
+ - 0.00916362466503102
+ - -0.003230266751149847
+ - -0.00266623735168524
+ - -0.002049904898263026
+ - -0.001425113759580535
+ - -0.0008170596255545296
+ - -0.0004633136340327531
+ - -0.0002653315937816079
+ - -0.0001362576528978232
+ - -3.414047865013824e-05
+ - -2.686708261983606e-06
+ - -1.932265661654438e-06
+ eef_cop_0:
+ -
+ - 0.01219661139342332
+ - 0.005561808272756302
+ - -0.002305134399572804
+ - -0.01106637777486117
+ - -0.02037504208130167
+ - -0.02918239447746913
+ - -0.02999797459250836
+ - -0.02999921472338266
+ - -0.0299997620036729
+ - -0.006539919121609756
+ - 0.01756721107780173
+ - 0.02999900185514049
+ - 0.02999966515287635
+ - 0.02999986283608619
+ - -0.03000001330748549
+ - -0.03000001365932108
+ - -0.0284924543073625
+ - -0.005765506653969564
+ - 0.0115230135247023
+ - 0.02350139580432383
+ - 0.02999969331091059
+ - 0.02999996995710729
+ - 0.02933581516991379
+ - 0.02154266856913652
+ - 0.008577277184584004
+ - -0.009650382034017404
+ - -0.02999997675048814
+ - -0.03000001554661627
+ - -0.03000001887441548
+ - 0.02999779311139069
+ - 0.0299914485832
+ - 0.002977737053596436
+ - -0.02081372256186605
+ - -0.02999979868514972
+ - -0.02690581424405139
+ - -0.02337846972672245
+ - 0.01416096510295727
+ - 0.02986463875918624
+ - 0.02999878910823275
+ - -0.03000001816876579
+ - -0.03000001614362967
+ - -0.0234068813863047
+ - -0.004968151828563821
+ - 0.009262385959506441
+ - 0.01970395288190376
+ - 0.02675014451005348
+ - 0.02999991504190783
+ - 0.02999998698120637
+ - 0.02999998622277799
+ - 0.02999993813254245
+ - 0.02814711104622746
+ - 0.02338791239565952
+ - 0.01689893537557573
+ - 0.008768856106188437
+ - -0.000924835918823654
+ - -0.01211530962117062
+ - -0.02474518418136307
+ - -0.03000000834730921
+ - -0.03000001612397576
+ - 0.02999998541020531
+ - 0.02999998541611805
+ - 0.02999997903888692
+ - 0.02999996240267338
+ - 0.02999992392284821
+ - 0.02999985730683187
+ - 0.02999973715532905
+ - 0.02999947919899742
+ - 0.02999670212997697
+ - 0.02924695273065238
+ - 0.02830015056710811
+ -
+ - -0.002425578153757311
+ - -0.00200078011951814
+ - -0.001126207155338259
+ - 0.0002491647745299728
+ - 0.002034264434789951
+ - 0.004184514374387949
+ - 0.005079168454092882
+ - 0.007053861098478896
+ - 0.009403075959112517
+ - -0.02513961719272605
+ - -0.02205971703261347
+ - -0.02340167466567751
+ - -0.03753612776134701
+ - -0.06758346436922476
+ - -0.08000001721636769
+ - -0.07999993488557781
+ - -0.06725179104903198
+ - -0.05360618305287188
+ - -0.04027225732340218
+ - -0.0272115173976095
+ - -0.01438585964714984
+ - -0.001756995970424754
+ - 0.01070780692319488
+ - 0.02305119899048633
+ - 0.03530851953748935
+ - 0.04751670489126204
+ - 0.05971269484571611
+ - 0.0719388241108837
+ - 0.07999998744509494
+ - 0.03333538955211365
+ - 0.01939104111404681
+ - 0.01751586288934683
+ - 0.03117236402472435
+ - 0.0799999948524958
+ - -0.01952296712055524
+ - -0.01487720905194704
+ - -0.01061193703401189
+ - -0.0164968866964701
+ - -0.06715809103265598
+ - -0.0800000175665717
+ - -0.07844186384319315
+ - -0.06660110501805064
+ - -0.05534214619990798
+ - -0.04461074121956243
+ - -0.03434401400373744
+ - -0.024479683372922
+ - -0.01495598051411653
+ - -0.005711321004200772
+ - 0.003315653559971142
+ - 0.01218599576617112
+ - 0.02096085118097014
+ - 0.02970155876371875
+ - 0.03846975877604137
+ - 0.04732747170613022
+ - 0.05633727572755221
+ - 0.0655624803120837
+ - 0.07506731934708115
+ - 0.07999999551765608
+ - 0.08000001882624712
+ - 0.06495410001252844
+ - 0.06641929268610682
+ - 0.06832036762544183
+ - 0.07147601065744898
+ - 0.07507733022272688
+ - 0.07872919247184289
+ - 0.07999483908094134
+ - 0.07998947995390913
+ - 0.07991949610097529
+ - 0.0788115868846293
+ - 0.07911991909525173
+ eef_frc_1:
+ -
+ - -0.01076212754525999
+ - -0.008753182586037099
+ - -0.006844399576939803
+ - -0.005057286589381011
+ - -0.003315908398839822
+ - -0.001525477855239159
+ - 0.002308224827369266
+ - 0.005244724407071521
+ - 0.009391309598651401
+ - 0.01308864794552024
+ - 0.0173161973620163
+ - 0.02165921344187565
+ - 0.02614665004118173
+ - 0.03080842607203729
+ - 0.03567562347684024
+ - 0.04282935268990259
+ - 0.06757044573178472
+ - 0.1046064159379892
+ - 0.1591476189830708
+ - -0.0327351615118134
+ - -0.02655229783049132
+ - -0.005207027660878095
+ - 0.0299032764143322
+ - 0.06206656767615802
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - -0.07675085632493006
+ - -0.02749976461482878
+ - 0.003132906177302972
+ - 0.04937966235170871
+ - 0.1040472987423471
+ - 0.1418995549445796
+ - 0.09408754867321162
+ - 0.06351909125625281
+ - 0.05965893410524935
+ - 0.05619635855045353
+ - 0.05310820190064915
+ - 0.0467749374532772
+ - 0.04312070898623311
+ - 0.04646042104513749
+ - 0.04621820987320404
+ - 0.04625964843664358
+ - 0.04660919130396179
+ - 0.04837191924303517
+ - 0.08640382032174099
+ - 0.1396958311796968
+ - -0.04076869300990743
+ - -0.002924642185048669
+ - 0.008219148956858173
+ - 0.03107477506921469
+ - 0.06879988277679859
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - -0.02021982594207746
+ - -0.01271197459268379
+ - -0.006585459354163598
+ - -0.001776035866508624
+ - 0.002410782672970355
+ - 0.007565690643647909
+ - 0.01094087034654372
+ - 0.01207704182048008
+ - 0.01394249160757461
+ - 0.01522624099551798
+ - 0.01749814331817114
+ -
+ - 0.001613367411107101
+ - 0.001799004355637918
+ - 0.001987632205232162
+ - 0.002179007373364955
+ - 0.002373126197774059
+ - 0.002571907218327445
+ - 0.002990774359852015
+ - 0.003458913897742034
+ - 0.003829365555667477
+ - 0.006592638633452188
+ - 0.006990434715945385
+ - 0.007392893151983126
+ - 0.007800280871360353
+ - 0.008212867524532033
+ - 0.008630926022211051
+ - 0.009054769422052833
+ - 0.009484894309008039
+ - 0.01284000290656166
+ - 0.02753277159057326
+ - -0.019991220164689
+ - -0.01447713880056615
+ - -0.003139308393294819
+ - 0.01284069928502479
+ - 0.01767753735166173
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0.01996606567464465
+ - 0.004676738799688872
+ - -0.003707665561466035
+ - -0.014299009409418
+ - -0.01558993385305209
+ - -0.006788174505917231
+ - -0.001271639297734862
+ - -0.0009479727954978695
+ - -0.0006249613252676937
+ - -0.0003023621623241151
+ - 2.00398789404638e-05
+ - 0.000342435145485548
+ - 0.0006650622860043185
+ - 0.0009882031603193522
+ - 0.001311954993057899
+ - 0.001636578525542435
+ - 0.00196228967358109
+ - 0.002289324790269909
+ - 0.00261857068595688
+ - 0.0165828678705822
+ - -0.02869956257700394
+ - -0.008371309871075193
+ - 0.0006109770991684596
+ - 0.01437386972299927
+ - 0.0166074366963542
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0.008210564448717613
+ - 0.005671126246677087
+ - 0.003178427176804499
+ - 0.0008034265892337581
+ - -0.001638133485670287
+ - -0.004692544754109644
+ - -0.007011467109900056
+ - -0.008327753504069287
+ - -0.009997829766852678
+ - -0.01124059933365514
+ - -0.01288662567855895
+ -
+ - 0.4971414569849972
+ - 0.4976331315683326
+ - 0.4980951914622541
+ - 0.4984679548534998
+ - 0.4987014611632928
+ - 0.4990768454783383
+ - 0.54450748472767
+ - 0.5938766514138824
+ - 0.6348108239065352
+ - 0.9999754770777017
+ - 1.000006669093992
+ - 1.000037688534311
+ - 1.000068512156763
+ - 1.000099115150567
+ - 1.000129476048448
+ - 1.000074984282721
+ - 0.9987368420930498
+ - 0.9944461651391855
+ - 0.9820940558771097
+ - 0.6735430631224754
+ - 0.6080824618188815
+ - 0.5279161623340927
+ - 0.4278595114662377
+ - 0.3287169544003313
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0.3581444579952473
+ - 0.4591065527659104
+ - 0.5363979181125951
+ - 0.5971204369990064
+ - 0.6803726863321222
+ - 0.9955729401632446
+ - 1.005498835425423
+ - 1.007336773980781
+ - 1.006516275333618
+ - 1.005699841106171
+ - 1.004886848121147
+ - 1.004243774277329
+ - 1.003474419265417
+ - 1.00243094984277
+ - 1.001551041758333
+ - 1.000672780060142
+ - 0.9997945046158432
+ - 0.9988633418009577
+ - 0.9945306777061554
+ - 0.9828718703902063
+ - 0.628435807725284
+ - 0.54306128158106
+ - 0.4981333900399162
+ - 0.4348864413632194
+ - 0.3269643005137183
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0.2195076894340146
+ - 0.2686161838221442
+ - 0.3147651161850024
+ - 0.3572697466816853
+ - 0.3957236017204772
+ - 0.4303757276439402
+ - 0.4608608781847216
+ - 0.4865593404716185
+ - 0.506620863038574
+ - 0.5199609463780214
+ - 0.5259751805455045
+ eef_trq_1:
+ -
+ - 4.915746722682453e-07
+ - 5.070050348324143e-07
+ - 5.564290190464287e-07
+ - 6.497545803236095e-07
+ - 8.044687509494479e-07
+ - 1.039575035985877e-06
+ - -2.372981323399444e-06
+ - -9.025845255356346e-06
+ - -3.606343397528267e-05
+ - -0.0009159969572097193
+ - -0.001089115604865459
+ - -0.00126922011573444
+ - -0.001456871184846412
+ - -0.001652205697718537
+ - -0.001854739190174462
+ - -0.001986818092187038
+ - -0.001233202491379769
+ - -0.00257556008967644
+ - -0.02061918022156732
+ - 7.400174073529116e-07
+ - 3.124841104411235e-07
+ - -0.0004465990284866111
+ - -0.001007148563235696
+ - -0.001035023883475967
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - -0.0002045706386112261
+ - -4.037448147071539e-05
+ - -2.745742261182908e-07
+ - 4.331297970468043e-07
+ - 1.23806907009787e-06
+ - -0.04275459674262772
+ - 1.280822578381951e-05
+ - 0.0002627373488796841
+ - 0.0001039512132239906
+ - -6.191118046215344e-05
+ - -0.000234523516508127
+ - -0.0004180696019792495
+ - -0.0006180533971148397
+ - -0.000798386373522026
+ - -0.00100354310386595
+ - -0.001219520809851255
+ - -0.001447858931377597
+ - -0.001677099162883775
+ - -0.001288764000648224
+ - 0.06319616273492179
+ - -6.13813849905608e-07
+ - -6.517755530620006e-07
+ - -2.97176280485664e-07
+ - -7.519114520625813e-06
+ - -0.0004474263708346508
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+ - -0.003230266751149847
+ - -0.00266623735168524
+ - -0.002049904898263026
+ - -0.001425113759580535
+ - -0.0008170596255545296
+ - -0.0004633136340327531
+ - -0.0002653315937816079
+ - -0.0001362576528978232
+ - -3.414047865013824e-05
+ - -2.686708261983606e-06
+ - -1.932265661654438e-06
+ eef_cop_1:
+ -
+ - 0.01331453539853417
+ - 0.006830153593023952
+ - -0.0008586372701324743
+ - -0.00939953940998328
+ - -0.0184575521354375
+ - -0.02829594913671194
+ - -0.02999830183619758
+ - -0.02999946162796778
+ - -0.02999987046710085
+ - 0.0245293661713863
+ - 0.01912857558482928
+ - 0.01577315760794133
+ - 0.01488265761091331
+ - 0.01689303220197278
+ - 0.02225953589554265
+ - 0.02999993118319662
+ - 0.0300000145245461
+ - 0.03000002076693203
+ - 0.03000002368431585
+ - -0.01055851038092853
+ - 0.01429142819842745
+ - 0.02999918374360178
+ - 0.02999960198521955
+ - 0.03000001514449093
+ - 0.02999718022002057
+ - 0.02999216081635914
+ - 0.01246638345704752
+ - -0.01832506396214896
+ - -0.02604049623159247
+ - 0.03000002601294154
+ - 0.03000001934901439
+ - 0.02130554239043448
+ - -0.000595037881784272
+ - -0.01663123500311335
+ - -0.02709050212043418
+ - -0.02999998212304503
+ - -0.0299999937076566
+ - -0.0299936485256676
+ - -0.02210960060710667
+ - -0.009358847118834826
+ - 0.008260828557702753
+ - 0.02999985709263405
+ - 0.03000001633497234
+ - 0.03000001816089007
+ - -0.02927463815489123
+ - -0.02663623663967054
+ - 0.00397849140300879
+ - 0.02980624907736372
+ - -0.0008543781824151395
+ - 0.02999992039506616
+ - 0.02999993733358325
+ - 0.02999993825279841
+ - 0.02999992479041916
+ - 0.02999988581352436
+ - 0.02999982058488868
+ - 0.02999970933624189
+ - 0.02999947738629556
+ - 0.02999761628596725
+ - 0.02986610803485484
+ - 0.02955547395865273
+ -
+ - -0.00150571181984365
+ - -0.001139466786670009
+ - -0.0004568028841087008
+ - 0.000564383290167509
+ - 0.002089355301290122
+ - 0.004236807133570741
+ - 0.008405243209222342
+ - 0.01224867766813731
+ - 0.01663805371372868
+ - 0.01849484599101306
+ - 0.02368061575046847
+ - 0.02959111092843324
+ - 0.03626673564225735
+ - 0.04374856377556674
+ - 0.05207840663781319
+ - 0.06129841339012716
+ - 0.0714448592323427
+ - 0.0799999593201986
+ - 0.08000001351028228
+ - 0.04020073389528084
+ - 0.03976258735049853
+ - 0.03460879592513224
+ - 0.03299826331863486
+ - 0.08000000754710485
+ - -0.02522277112028661
+ - -0.02809699082269202
+ - -0.03338785005722311
+ - -0.04452918795587905
+ - -0.06976476566355037
+ - -0.08000000440638859
+ - -0.07250571642473284
+ - -0.06014408876301392
+ - -0.04785549738650278
+ - -0.03560093574024756
+ - -0.02335090557012407
+ - -0.0110761376504089
+ - 0.001253503120564492
+ - 0.0136701648490591
+ - 0.02620150821094907
+ - 0.03887881879928584
+ - 0.05173277533671566
+ - 0.06479519213228906
+ - 0.07812539819917473
+ - 0.08000001140746978
+ - 0.031153996253482
+ - 0.01183503153415755
+ - 0.01266119705456387
+ - 0.01137303718822037
+ - 0.0799999897069948
+ - 0.003943204586966992
+ - 0.01921153810462954
+ - 0.03128294103967214
+ - 0.03987773332089265
+ - 0.04723105820167561
+ - 0.05326968997722614
+ - 0.06072482452642763
+ - 0.06772841061862633
+ - 0.07291225178137332
+ - 0.07744972982468096
+ - 0.07907854985697448
+cntopt_params:
+ num_contacts: [3, 3, 0, 0]
+ eefcnt_rf:
+ cnt0: [0.0, 1.0, 0.086, 0.15, -0.92, 1.0, 0.0, 0.0, 0.0, 1.0]
+ cnt1: [2.0, 4.5, 0.500, 0.45, -0.76, 0.9689, 0.0, -0.2474, 0.0, 1.0]
+ cnt2: [6.0, 9.9, 0.450, 0.98, -0.27, 1.0, 0.0, 0.0, 0.0, 1.0]
+ eefcnt_lf:
+ cnt0: [0.0, 2.5, -0.086, 0.15, -0.92, 1.0, 0.0, 0.0, 0.0, 1.0]
+ cnt1: [4.0, 6.5, -0.080, 0.70, -0.52, 0.96891, 0.0, 0.2474, 0.0, 1.0]
+ cnt2: [8.5, 9.9, -0.080, 1.25, -0.25, 1.0, 0.0, 0.0, 0.0, 1.0]
+ eefcnt_rh: ~
+ eefcnt_lh: ~
\ No newline at end of file
diff --git a/Config/Draco/DEFAULT_CONIC_SOLVER_SETTING.yaml b/Config/Draco/DEFAULT_CONIC_SOLVER_SETTING.yaml
new file mode 100644
index 00000000..08aa510c
--- /dev/null
+++ b/Config/Draco/DEFAULT_CONIC_SOLVER_SETTING.yaml
@@ -0,0 +1,54 @@
+solver_variables:
+
+ ##########################
+ # Convergence tolerances #
+ ##########################
+
+ feasibility_tolerance: 1e-7
+ absolute_suboptimality_gap: 1e-7
+ relative_suboptimality_gap: 1e-6
+ feasibility_tolerance_inaccurate: 1e-4
+ absolute_suboptimality_gap_inaccurate: 5e-5
+ relative_suboptimality_gap_inaccurate: 5e-5
+
+ ############################
+ # Equilibration parameters #
+ ############################
+
+ equil_iterations: 3
+
+ ############################
+ # Linear System parameters #
+ ############################
+
+ dyn_reg_thresh: 1e-13
+ lin_sys_accuracy: 1e-14
+ err_reduction_factor: 6.0
+ num_iter_ref_lin_solve: 9
+ static_regularization: 7e-8
+ dynamic_regularization: 2e-7
+
+ ########################
+ # Algorithm parameters #
+ ########################
+
+ safeguard: 500.0
+ min_step_length: 1e-6
+ max_step_length: 0.999
+ min_centering_step: 1e-4
+ max_centering_step: 1.00
+ step_length_scaling: 0.99
+
+ ####################
+ # Model parameters #
+ ####################
+
+ verbose: False
+
+ max_iters: 100
+ ipsolver_max_iters: 100
+ ipsolver_warm_iters: 100
+
+ num_itrefs_trustregion: 3
+ trust_region_threshold: 0.15
+ soft_constraint_weight: 1.0e4
\ No newline at end of file
diff --git a/ExternalSource/Optimizer/CMakeLists.txt b/ExternalSource/Optimizer/CMakeLists.txt
index 47b97985..bee4cd93 100755
--- a/ExternalSource/Optimizer/CMakeLists.txt
+++ b/ExternalSource/Optimizer/CMakeLists.txt
@@ -1 +1,2 @@
add_subdirectory(Goldfarb)
+add_subdirectory(ConicSolver)
diff --git a/ExternalSource/Optimizer/ConicSolver/CMakeLists.txt b/ExternalSource/Optimizer/ConicSolver/CMakeLists.txt
new file mode 100644
index 00000000..2291b7aa
--- /dev/null
+++ b/ExternalSource/Optimizer/ConicSolver/CMakeLists.txt
@@ -0,0 +1,3 @@
+file(GLOB_RECURSE srcs "*.cpp" "*.hpp")
+
+add_library(myConicSolver ${srcs})
diff --git a/ExternalSource/Optimizer/ConicSolver/include/solver/interface/Cone.hpp b/ExternalSource/Optimizer/ConicSolver/include/solver/interface/Cone.hpp
new file mode 100644
index 00000000..114b6573
--- /dev/null
+++ b/ExternalSource/Optimizer/ConicSolver/include/solver/interface/Cone.hpp
@@ -0,0 +1,450 @@
+/*
+ *
+ * ECOS - Embedded Conic Solver
+ * Copyright (C) [2012-2015] A. Domahidi [domahidi@embotech.com],
+ * Automatic Control Lab, ETH Zurich & embotech GmbH, Zurich, Switzerland.
+ *
+ * Copyright [2017] Max Planck Society. All rights reserved.
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see .
+ */
+
+#pragma once
+
+#include
+#include
+#include
+#include
+#include
+
+namespace solver {
+
+ class Cone;
+ class Vector;
+ class ConicVector;
+ class ExtendedVector;
+ class OptimizationVector;
+
+ class ScalingOperator
+ {
+ public:
+ ScalingOperator(){}
+ ~ScalingOperator(){}
+
+ void initialize(const Cone& cone) { cone_ = &cone; }
+ ConicVector operator*(const Eigen::Ref& rhs) const;
+
+ private:
+ const Cone* cone_;
+ };
+
+ /**
+ * This class performs appropriate scalings of the cone variables.
+ * Details can be found in the paper: Domahidi, A. and Chu, E. and Boyd, S.,
+ * ECOS: An SOCP solver for embedded systems, ECC 2013, pages 3071-3076
+ */
+ class NesterovToddScaling
+ {
+ public:
+ friend class Cone;
+
+ NesterovToddScaling(int conesize);
+ ~NesterovToddScaling(){}
+
+ inline double& w() { return w_; }
+ inline double& d1() { return d1_; }
+ inline double& u0() { return u0_; }
+ inline double& u1() { return u1_; }
+ inline double& v1() { return v1_; }
+ inline double& eta() { return eta_; }
+ inline double& etaSquare() { return eta_square_; }
+ int& indexSoc(int id) { return SOC_id_(id); }
+ Eigen::VectorXd& scalingSoc() { return wbar_; }
+ double& scalingSoc(int id) { return wbar_(id); }
+
+ inline const double& w() const { return w_; }
+ inline const double& d1() const { return d1_; }
+ inline const double& u0() const { return u0_; }
+ inline const double& u1() const { return u1_; }
+ inline const double& v1() const { return v1_; }
+ inline const double& eta() const { return eta_; }
+ inline const double& etaSquare() const { return eta_square_; }
+ const int& indexSoc(int id) const { return SOC_id_(id); }
+ const Eigen::VectorXd& scalingSoc() const { return wbar_; }
+ const double& scalingSoc(int id) const { return wbar_(id); }
+
+ private:
+ Eigen::VectorXd wbar_;
+ Eigen::VectorXi SOC_id_;
+ double w_, d1_, u0_, u1_, v1_, eta_, eta_square_;
+ };
+
+ /**
+ * This class contains all information about the conic optimization
+ * problem, and provides functionality for conic operations, such as
+ * scalings, residuals and sizes.
+ */
+ class Cone
+ {
+ public:
+ // Cone class
+ Cone(){}
+ ~Cone(){}
+
+ // Setting up problem
+ void setupLpcone(int size);
+ void setupSocone(const Eigen::VectorXi& indices);
+ void initialize(int nvars, int nleq, int nlineq, const Eigen::VectorXi& nsoc);
+
+ // Getter and setter methods for problem size variables
+ inline const int numLeq() const { return neq_; }
+ inline const int numSoc() const { return nsoc_; }
+ inline const int sizeSoc() const { return ssoc_; }
+ inline const int sizeLpc() const { return nineq_; }
+ inline const int numVars() const { return nvars_; }
+ inline const int sizeCone() const { return sizecone_; }
+ inline const int extSizeSoc() const { return extssoc_; }
+ inline const int sizeSoc(int id) const { return q_(id); }
+ inline const int numCones() const { return nineq_+nsoc_; }
+ inline const int sizeProb() const { return sizeproblem_; }
+ inline const int extSizeCone() const { return extsizecone_; }
+ inline const int lpConeStart() const { return lpconestart_; }
+ inline const int soConeStart() const { return soconestart_; }
+ inline const int extSizeProb() const { return extsizeproblem_; }
+ inline const int startSoc(int id) const { return SOC_start_(id); }
+ inline const int sizeConstraints() const { return sizeconstraints_; }
+ inline const int optStartSoc(int id) const { return opt_SOC_start_(id); }
+ inline const int extStartSoc(int id) const { return ext_SOC_start_(id); }
+ static double normSoc(const Eigen::Ref& v) { return v[0]-v.tail(v.size()-1).norm(); }
+
+ // Getter and setter methods for variables of linear cone
+ int& indexLpc(int id) { return LP_id_(id); }
+ Eigen::VectorXd& scalingLpc() { return LP_scaling_; }
+ double& scalingLpc(int id) { return LP_scaling_(id); }
+ Eigen::VectorXd& sqScalingLpc() { return LP_sq_scaling_; }
+ double& sqScalingLpc(int id) { return LP_sq_scaling_(id); }
+
+ const int& indexLpc(int id) const { return LP_id_(id); }
+ const Eigen::VectorXd& scalingLpc() const { return LP_scaling_; }
+ const double& scalingLpc(int id) const { return LP_scaling_(id); }
+ const Eigen::VectorXd& sqScalingLpc() const { return LP_sq_scaling_; }
+ const double& sqScalingLpc(int id) const { return LP_sq_scaling_(id); }
+
+ // Getter and setter methods for variables of second order cone
+ NesterovToddScaling& soc(int id) { return soc_[id]; }
+ const NesterovToddScaling& soc(int id) const { return soc_[id]; }
+ inline double conicResidual(const double* u, int size) const;
+ inline double conicResidual(const double* u, const double* v, int size) const;
+ double conicResidual(const Eigen::Ref& u) const;
+ double conicResidual(const Eigen::Ref& u, const Eigen::Ref& v) const;
+
+ // Functions for cones
+ double safeDivision(double x, double y) const;
+ void conicProjection(Eigen::Ref s);
+ void conicNTScaling(const double* z, double* lambda) const;
+ void conicNTScaling(const Eigen::VectorXd& z, Eigen::Ref lambda) const;
+ void conicNTScaling2(const Eigen::VectorXd& x, Eigen::Ref y) const;
+ void conicDivision(const Eigen::Ref& u, const Eigen::Ref& w, Eigen::Ref v) const;
+ double conicProduct(const Eigen::Ref u, const Eigen::Ref v, Eigen::Ref w) const;
+ ConeStatus updateNTScalings(const Eigen::VectorXd& s, const Eigen::VectorXd& z, Eigen::VectorXd& lambda);
+ void unpermuteSolution(const Eigen::PermutationMatrix& Pinv, const Eigen::VectorXd& Px, OptimizationVector& sd) const;
+ void unpermuteSolution(const Eigen::PermutationMatrix& Pinv, const Eigen::VectorXd& Px, OptimizationVector& sd, Eigen::VectorXd& dz) const;
+
+ // Getter and setter methods for scaling operators
+ ScalingOperator& W() { return W_scaling_operator_; }
+ const ScalingOperator& W() const { return W_scaling_operator_; }
+
+ private:
+ int nvars_; // number variables
+ int neq_; // number linear equality constraints
+ int nineq_; // number linear inequality constraints
+ int nsoc_; // number second order cone constraints
+ int ssoc_; // size second order cone constraints
+ int extssoc_; // extended size second order cone constraints
+ int lpconestart_; // offset to linear cone start
+ int soconestart_; // offset to second order cone start
+ int sizecone_; // size of linear and second order cones
+ int extsizecone_; // extended size of linear and second order cones
+ int sizeconstraints_; // size of the constraints
+ int sizeproblem_; // size of number of variables plus constraints
+ int extsizeproblem_; // size of number of variables plus constraints
+ Eigen::VectorXi q_; // sizes of second-order cone constraints
+
+ // variables of LP cone
+ Eigen::VectorXi LP_id_;
+ Eigen::VectorXd LP_scaling_;
+ Eigen::VectorXd LP_sq_scaling_;
+
+ // variables of SO cone
+ Eigen::VectorXd skbar_, zkbar_;
+ std::vector soc_;
+ Eigen::VectorXi SOC_start_, opt_SOC_start_, ext_SOC_start_;
+
+ // Operators
+ ScalingOperator W_scaling_operator_;
+ };
+
+ /**
+ * Helper class to work with optimization variables, lay down in normal order.
+ */
+ class Vector : public Eigen::VectorXd
+ {
+ public:
+ Vector() : Eigen::VectorXd() {}
+ ~Vector(){}
+
+ template
+ Vector(const Eigen::MatrixBase& other) : Eigen::VectorXd(other) {}
+
+ template
+ Vector& operator=(const Eigen::MatrixBase & other) {
+ this->Eigen::VectorXd::operator=(other);
+ return *this;
+ }
+
+ void initialize(const Cone& cone);
+
+ Eigen::Ref x() { return this->head(cone_->numVars()); }
+ Eigen::Ref y() { return this->segment(cone_->numVars(), cone_->numLeq()); }
+ Eigen::Ref z() { return this->segment(cone_->lpConeStart(), cone_->sizeCone()); }
+ Eigen::Ref yz() { return this->segment(cone_->numVars(), cone_->sizeConstraints()); }
+
+ Eigen::Ref zLpc() { return this->segment(cone_->lpConeStart(), cone_->sizeLpc()); }
+ Eigen::Ref zSoc() { return this->segment(cone_->soConeStart(), cone_->sizeSoc()); }
+ Eigen::Ref zSoc(int id) { return this->segment(cone_->optStartSoc(id), cone_->sizeSoc(id)); }
+
+ const Eigen::Ref x() const { return this->head(cone_->numVars()); }
+ const Eigen::Ref y() const { return this->segment(cone_->numVars(), cone_->numLeq()); }
+ const Eigen::Ref z() const { return this->segment(cone_->lpConeStart(), cone_->sizeCone()); }
+ const Eigen::Ref yz() const { return this->segment(cone_->numVars(), cone_->sizeConstraints()); }
+
+ const Eigen::Ref zLpc() const { return this->segment(cone_->lpConeStart(), cone_->sizeLpc()); }
+ const Eigen::Ref zSoc() const { return this->segment(cone_->soConeStart(), cone_->sizeSoc()); }
+ const Eigen::Ref zSoc(int id) const { return this->segment(cone_->optStartSoc(id), cone_->sizeSoc(id)); }
+
+ private:
+ const Cone* cone_;
+ };
+
+ /**
+ * Helper class to work with variables, which are members of a proper convex cone.
+ */
+ class ConicVector : public Eigen::VectorXd
+ {
+ public:
+ ConicVector() : Eigen::VectorXd() {}
+ ConicVector(int size) : Eigen::VectorXd(size) {}
+ ~ConicVector(){}
+
+ template
+ ConicVector(const Eigen::MatrixBase& other) : Eigen::VectorXd(other) {}
+
+ template
+ ConicVector& operator=(const Eigen::MatrixBase & other) {
+ this->Eigen::VectorXd::operator=(other);
+ return *this;
+ }
+
+ ConicVector& operator=(const ConicVector& other) {
+ this->Eigen::VectorXd::operator=(other);
+ this->cone_ = other.cone_;
+ return *this;
+ }
+
+ void initialize(const Cone& cone);
+ ConicVector operator/ (const ConicVector& rhs) const;
+ ConicVector operator* (const ConicVector& rhs) const;
+ ConicVector operator+ (const ConicVector& rhs) const;
+ ConicVector operator+ (const double& rhs) const;
+ ConicVector operator- (const double& rhs) const;
+ ConicVector& operator+=(const double& rhs);
+ ConicVector operator- () const;
+
+ Eigen::Ref z() { return this->head(cone_->sizeCone()); }
+ Eigen::Ref zLpc() { return this->head(cone_->sizeLpc()); }
+ Eigen::Ref zSoc() { return this->segment(cone_->sizeLpc(),cone_->sizeSoc()); }
+ Eigen::Ref zSoc(int id) { return this->segment(cone_->startSoc(id),cone_->sizeSoc(id)); }
+
+ const Eigen::Ref z() const { return this->head(cone_->sizeCone()); }
+ const Eigen::Ref zLpc() const { return this->head(cone_->sizeLpc()); }
+ const Eigen::Ref zSoc() const { return this->segment(cone_->sizeLpc(),cone_->sizeSoc()); }
+ const Eigen::Ref zSoc(int id) const { return this->segment(cone_->startSoc(id),cone_->sizeSoc(id)); }
+
+ private:
+ const Cone* cone_;
+ };
+
+ /**
+ * Helper class to work with optimization variables, lay down in an extended order.
+ */
+ class ExtendedVector : public Eigen::VectorXd
+ {
+ public:
+ ExtendedVector() : Eigen::VectorXd() {}
+ ~ExtendedVector(){}
+
+ template
+ ExtendedVector(const Eigen::MatrixBase& other) : Eigen::VectorXd(other) {}
+
+ template
+ ExtendedVector& operator=(const Eigen::MatrixBase & other) {
+ this->Eigen::VectorXd::operator=(other);
+ return *this;
+ }
+
+ void initialize(const Cone& cone);
+
+ void zRtoE(const Eigen::Ref& rvec);
+ Eigen::Ref x() { return this->head(cone_->numVars()); }
+ Eigen::Ref y() { return this->segment(cone_->numVars(),cone_->numLeq()); }
+ Eigen::Ref z() { return this->segment(cone_->lpConeStart(),cone_->extSizeCone()); }
+
+ Eigen::Ref zLpc() { return this->segment(cone_->lpConeStart(),cone_->sizeLpc()); }
+ Eigen::Ref zSoc() { return this->segment(cone_->soConeStart(),cone_->extSizeSoc()); }
+ Eigen::Ref zSoc(int id) { return this->segment(cone_->extStartSoc(id),cone_->sizeSoc(id)+2); }
+
+ const Eigen::Ref x() const { return this->head(cone_->numVars()); }
+ const Eigen::Ref y() const { return this->segment(cone_->numVars(),cone_->numLeq()); }
+ const Eigen::Ref z() const { return this->segment(cone_->lpConeStart(),cone_->extSizeCone()); }
+
+ const Eigen::Ref zLpc() const { return this->segment(cone_->lpConeStart(),cone_->sizeLpc()); }
+ const Eigen::Ref zSoc() const { return this->segment(cone_->soConeStart(),cone_->extSizeSoc()); }
+ const Eigen::Ref zSoc(int id) const { return this->segment(cone_->extStartSoc(id),cone_->sizeSoc(id)+2); }
+
+ private:
+ const Cone* cone_;
+ };
+
+ /**
+ * Helper class to define an optimization vector, including primal and dual
+ * variables, and variables to render the optimization problem homogeneous.
+ */
+ class OptimizationVector : public Eigen::VectorXd
+ {
+ public:
+ OptimizationVector() : Eigen::VectorXd() {}
+ ~OptimizationVector(){}
+
+ template
+ OptimizationVector(const Eigen::MatrixBase& other) : Eigen::VectorXd(other) {}
+
+ template
+ OptimizationVector& operator=(const Eigen::MatrixBase & other) {
+ this->Eigen::VectorXd::operator=(other);
+ this->cone_ = other.cone_;
+ return *this;
+ }
+
+ void initialize(const Cone& cone);
+
+ OptimizationVector operator+(const OptimizationVector& rhs) const;
+
+ Eigen::Ref x() { return this->head(cone_->numVars()); }
+ Eigen::Ref y() { return this->segment(cone_->numVars(),cone_->numLeq()); }
+ Eigen::Ref z() { return this->segment(cone_->lpConeStart(),cone_->sizeCone()); }
+ Eigen::Ref s() { return this->segment(cone_->sizeProb()+2,cone_->sizeCone()); }
+ Eigen::Ref yz() { return this->segment(cone_->numVars(),cone_->sizeConstraints()); }
+ Eigen::Ref yztau() { return this->segment(cone_->numVars(),cone_->sizeConstraints()+1); }
+ double& tau() { return this->segment<2>(cone_->sizeProb())[0]; }
+ double& kappa() { return this->segment<2>(cone_->sizeProb())[1]; }
+
+ const Eigen::Ref x() const { return this->head(cone_->numVars()); }
+ const Eigen::Ref y() const { return this->segment(cone_->numVars(),cone_->numLeq()); }
+ const Eigen::Ref s() const { return this->segment(cone_->lpConeStart(),cone_->sizeCone()); }
+ const Eigen::Ref z() const { return this->segment(cone_->sizeProb()+2,cone_->sizeCone()); }
+ const Eigen::Ref yz() const { return this->segment(cone_->numVars(),cone_->sizeConstraints()); }
+ const Eigen::Ref yztau() const { return this->segment(cone_->numVars(),cone_->sizeConstraints()+1); }
+ double tau() const { return this->segment<2>(cone_->sizeProb())[0]; }
+ double kappa() const { return this->segment<2>(cone_->sizeProb())[1]; }
+
+ Eigen::Ref xyz() { return this->head(cone_->sizeProb()); }
+ Eigen::Ref xyztau() { return this->head(cone_->sizeProb()+1); }
+ Eigen::Ref zLpc() { return this->segment(cone_->lpConeStart(),cone_->sizeLpc()); }
+ Eigen::Ref zSoc() { return this->segment(cone_->soConeStart(),cone_->sizeSoc()); }
+ Eigen::Ref zSoc(int id) { return this->segment(cone_->optStartSoc(id),cone_->sizeSoc(id)); }
+
+ const Eigen::Ref xyz() const { return this->head(cone_->sizeProb()); }
+ const Eigen::Ref xyztau() const { return this->head(cone_->sizeProb()+1); }
+ const Eigen::Ref zLpc() const { return this->segment(cone_->lpConeStart(),cone_->sizeLpc()); }
+ const Eigen::Ref zSoc() const { return this->segment(cone_->soConeStart(),cone_->sizeSoc()); }
+ const Eigen::Ref zSoc(int id) const { return this->segment(cone_->optStartSoc(id),cone_->sizeSoc(id)); }
+
+ private:
+ const Cone* cone_;
+ };
+
+ /**
+ * Class that provides storage space for the optimization matrices,
+ * vectors and variables.
+ */
+ class SolverStorage
+ {
+ public:
+ SolverStorage(){}
+ ~SolverStorage(){}
+
+ double& gTh() { return gTh_; }
+ const double& gTh() const { return gTh_; }
+
+ Eigen::SparseMatrix& A() { return A_; }
+ Eigen::SparseMatrix& G() { return G_; }
+ Eigen::SparseMatrix& At() { return At_; }
+ Eigen::SparseMatrix& Gt() { return Gt_; }
+ const Eigen::SparseMatrix& A() const { return A_; }
+ const Eigen::SparseMatrix& G() const { return G_; }
+ const Eigen::SparseMatrix& At() const { return At_; }
+ const Eigen::SparseMatrix& Gt() const { return Gt_; }
+
+ void initializeMatrices();
+ void initialize(Cone& cone, SolverSetting& stgs);
+ void cleanCoeffs() { Acoeffs_.clear(); Gcoeffs_.clear(); }
+ void addCoeff(const Eigen::Triplet& coeff, bool flag_eq = false);
+
+ Eigen::Ref cbh() { return cbh_; }
+ Eigen::Ref c() { return cbh_.x(); }
+ Eigen::Ref b() { return cbh_.y(); }
+ Eigen::Ref h() { return cbh_.z(); }
+ Eigen::Ref bh() { return cbh_.yz(); }
+ std::vector>& Acoeffs() { return Acoeffs_; }
+ std::vector>& Gcoeffs() { return Gcoeffs_; }
+ const Eigen::Ref cbh() const { return cbh_; }
+ const Eigen::Ref c() const { return cbh_.x(); }
+ const Eigen::Ref b() const { return cbh_.y(); }
+ const Eigen::Ref h() const { return cbh_.z(); }
+ const Eigen::Ref bh() const { return cbh_.yz(); }
+ const std::vector>& Acoeffs() const { return Acoeffs_; }
+ const std::vector>& Gcoeffs() const { return Gcoeffs_; }
+
+ OptimizationVector& u() { return u_opt_; }
+ OptimizationVector& v() { return v_opt_; }
+ OptimizationVector& ut() { return u_t_opt_; }
+ OptimizationVector& uprev() { return u_prev_opt_; }
+ Vector& cbh_copy() { return cbh_copy_; }
+ const OptimizationVector& u() const { return u_opt_; }
+ const OptimizationVector& v() const { return v_opt_; }
+ const OptimizationVector& ut() const { return u_t_opt_; }
+ const OptimizationVector& uprev() const { return u_prev_opt_; }
+ const Vector& cbh_copy() const { return cbh_copy_; }
+
+ private:
+ double gTh_;
+ Cone* cone_;
+ SolverSetting* stgs_;
+ Vector cbh_, cbh_copy_;
+ Eigen::SparseMatrix A_, At_, G_, Gt_;
+ std::vector> Acoeffs_, Gcoeffs_;
+ OptimizationVector u_opt_, v_opt_, u_t_opt_, u_prev_opt_;
+ };
+
+}
diff --git a/ExternalSource/Optimizer/ConicSolver/include/solver/interface/Exprs.hpp b/ExternalSource/Optimizer/ConicSolver/include/solver/interface/Exprs.hpp
new file mode 100644
index 00000000..97c01cca
--- /dev/null
+++ b/ExternalSource/Optimizer/ConicSolver/include/solver/interface/Exprs.hpp
@@ -0,0 +1,96 @@
+/*
+ * Copyright [2017] Max Planck Society. All rights reserved.
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see .
+ */
+
+#pragma once
+
+#include
+#include
+
+namespace solver {
+
+ /**
+ * Helper class to ease the construction of a linear expression (e.g. a*x = b)
+ */
+ class LinExpr
+ {
+ public:
+ LinExpr(double constant=0.0) { constant_ = constant; vars_.clear(); coeffs_.clear(); }
+ LinExpr(Var var, double coeff=1.0) { constant_ = 0.0; vars_.push_back(var); coeffs_.push_back(coeff); }
+
+ double getValue() const;
+ static bool isClean(const LinExpr& rhs);
+ static LinExpr clean(const LinExpr& rhs);
+ int size() const { return vars_.size(); }
+ double& getConstant() { return constant_; }
+ Var getVar(int i) const { return vars_[i]; }
+ double getCoeff(int i) const { return coeffs_[i]; }
+ const double& getConstant() const { return constant_; }
+ void clear() { constant_ = 0.0; coeffs_.clear(); vars_.clear(); }
+
+ LinExpr operator*(double factor) const;
+ LinExpr& operator+=(const LinExpr& rhs);
+ LinExpr operator+(const LinExpr& rhs) const { LinExpr result = *this; result += rhs; return result; }
+ LinExpr operator-(const LinExpr& rhs) const { LinExpr result = *this; result += rhs*(-1.0); return result; }
+ LinExpr operator=(const LinExpr& rhs) { constant_ = rhs.constant_; vars_ = rhs.vars_; coeffs_ = rhs.coeffs_; return *this; }
+
+ private:
+ double constant_;
+ std::vector vars_;
+ std::vector coeffs_;
+ };
+
+ /**
+ * Helper class to ease the construction of a quadratic expression
+ * (e.g. sum_i (a_i*x + b_i)^2 + (c*x + d))
+ */
+ class DCPQuadExpr
+ {
+ public:
+ DCPQuadExpr() : lexpr_(0.0), trust_region_(false), soft_constraint_(false) {}
+ ~DCPQuadExpr(){}
+
+ double getValue() const;
+ void addLinTerm(const LinExpr& lexpr) { lexpr_ += lexpr; }
+ void clear() { lexpr_ = 0.0; qexpr_.clear(); coeffs_.clear(); extra_vars_.clear(); }
+ void addQuaTerm(double coeff, const LinExpr& lexpr) { if (coeff != 0.0) { qexpr_.push_back(lexpr); coeffs_.push_back(coeff); } }
+
+ LinExpr& lexpr() { return lexpr_; }
+ std::string& sense() { return sense_; }
+ bool& trustRegion() { return trust_region_; }
+ std::vector& qexpr() { return qexpr_; }
+ std::vector& coeffs() { return coeffs_; }
+ bool& softConstraint() { return soft_constraint_; }
+ std::vector& extraVars() { return extra_vars_; }
+
+ const LinExpr& lexpr() const { return lexpr_; }
+ const std::string& sense() const { return sense_; }
+ const bool& trustRegion() const { return trust_region_; }
+ const std::vector& qexpr() const { return qexpr_; }
+ const std::vector& coeffs() const { return coeffs_; }
+ const bool& softConstraint() const { return soft_constraint_; }
+ const std::vector& extraVars() const { return extra_vars_; }
+ const Var getVar(int qid, int lid) const { return qexpr_[qid].getVar(lid); }
+
+ private:
+ LinExpr lexpr_;
+ std::string sense_;
+ std::vector coeffs_;
+ std::vector qexpr_;
+ std::vector extra_vars_;
+ bool trust_region_, soft_constraint_;
+ };
+}
diff --git a/ExternalSource/Optimizer/ConicSolver/include/solver/interface/Model.hpp b/ExternalSource/Optimizer/ConicSolver/include/solver/interface/Model.hpp
new file mode 100644
index 00000000..aa5adf69
--- /dev/null
+++ b/ExternalSource/Optimizer/ConicSolver/include/solver/interface/Model.hpp
@@ -0,0 +1,69 @@
+/*
+ * Copyright [2017] Max Planck Society. All rights reserved.
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see .
+ */
+
+#pragma once
+
+#include
+#include
+
+namespace solver {
+
+ /**
+ * Main class to construct a second-order cone optimization problem.
+ * It provides functionality for defining an objective function, linear
+ * equality and inequality constraints, second-order cone constraints
+ * and heuristics for solving problems with non-convex quadratic constraints.
+ */
+ class Model
+ {
+ public:
+ Model(){}
+ ~Model(){}
+
+ void clean();
+ SolverSetting& getStgs() { return stgs_; }
+ const SolverSetting& getStgs() const { return stgs_; }
+ Var addVar(const VarType& type, double lb, double ub, double guess=0.0);
+ void addLinConstr(const LinExpr& lhs, const std::string sense, const LinExpr& rhs);
+ void addSocConstr(const DCPQuadExpr& qexpr, const std::string sense, const LinExpr& lexpr);
+ void addQuaConstr(const DCPQuadExpr& qexpr, const std::string sense, const LinExpr& expr, const QuadConstrApprox& qapprox = QuadConstrApprox::None );
+ void configSetting(const std::string cfg_file, const std::string stgs_vars_yaml = "solver_variables");
+ void setObjective(const DCPQuadExpr& qexpr, const LinExpr& expr);
+ ExitCode optimize();
+
+ private:
+ Cone& getCone() { return cone_; }
+ SolverStorage& getStorage() { return stg_; }
+ const Cone& getCone() const { return cone_; }
+ const SolverStorage& getStorage() const { return stg_; }
+ ExitCode solve_problem();
+ void build_problem(int iter_id, bool warm_start = false);
+
+ private:
+ Cone cone_;
+ SolverStorage stg_;
+ SolverSetting stgs_;
+ InteriorPointSolver ip_solver_;
+
+ DCPQuadExpr objective_;
+ std::vector > vars_;
+ int numTrustRegions_, numSoftConstraints_;
+ std::vector leqcons_, lineqcons_;
+ std::vector qineqcons_, soccons_;
+
+ };
+}
diff --git a/ExternalSource/Optimizer/ConicSolver/include/solver/interface/OptVar.hpp b/ExternalSource/Optimizer/ConicSolver/include/solver/interface/OptVar.hpp
new file mode 100644
index 00000000..15640e58
--- /dev/null
+++ b/ExternalSource/Optimizer/ConicSolver/include/solver/interface/OptVar.hpp
@@ -0,0 +1,77 @@
+/*
+ * Copyright [2017] Max Planck Society. All rights reserved.
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see .
+ */
+
+#pragma once
+
+#include
+#include
+
+namespace solver {
+
+ /** Class that contains all information about the optimization variables
+ * such as upper and lower bounds, initial guess if any, and some
+ * auxiliary functions for easier handling.
+ */
+ class OptimizationVariable
+ {
+ public:
+ typedef Eigen::Matrix OptVector;
+ typedef Eigen::Matrix OptMatrix;
+
+ public:
+ //! Class constructor and destructor
+ OptimizationVariable() : guessValueInitialized_(false) {}
+ ~OptimizationVariable() {}
+
+ //! Some alternative initialization functions
+ void initialize(const char& type, int rows, int cols, double lBnd, double uBnd, int& startIndexInOptVec );
+ void initialize(const char& type, int rows, int cols, OptVector& lBnd, OptVector& uBnd, int& startIndexInOptVec );
+ void initialize(const char& type, int rows, int cols, OptMatrix& lBnd, OptMatrix& uBnd, int& startIndexInOptVec );
+ void initialize(const char& type, int rows, int cols, double lBnd, double uBnd, int& startIndexInOptVec, double guess );
+ void initialize(const char& type, int rows, int cols, double lBnd, double uBnd, int& startIndexInOptVec, OptMatrix& guess );
+ void initialize(const char& type, int rows, int cols, OptVector& lBnd, OptVector& uBnd, int& startIndexInOptVec, OptMatrix& guess );
+ void initialize(const char& type, int rows, int cols, OptMatrix& lBnd, OptMatrix& uBnd, int& startIndexInOptVec, OptMatrix& guess );
+
+ //! Some helper functions to construct the problem
+ int id(int row, int col) const { return indexPosition_ + this->getColBndInd(col)*rows_ + this->getRowBndInd(row); }
+ void constraintIndexToCurrentValue(int index) { lBndMat_.col(index) = guessMat_.col(index); uBndMat_.col(index) = guessMat_.col(index); }
+ void setAndConstraintIndexToCurrentValue(int index, double value) { guessMat_.col(index).setConstant(value); this->constraintIndexToCurrentValue(index); }
+ void setAndConstraintIndexToCurrentValue(int index, OptVector& value) { guessMat_.col(index) = value; this->constraintIndexToCurrentValue(index); }
+ void getValues(OptMatrix& lBnd, OptMatrix& uBnd, OptMatrix& guess, int& size, char& type) const { lBnd = lBndMat_; uBnd = uBndMat_; guess = guessMat_; size = this->getNumElements(); type = type_; }
+
+ //! Some getter and setter methods
+ int getNumRows() const { return rows_; }
+ int getNumCols() const { return cols_; }
+ int getStartIndex() const { return indexPosition_; }
+ int getNumElements() const { return rows_*cols_; }
+ void getGuessValue(OptMatrix& guess) const { guess.resize(rows_, cols_); guess = guessMat_; }
+ void getGuessValueByCol(int index, OptVector& guess) const { guess.resize(rows_,1); guess = guessMat_.col(index); }
+ void setGuessValue(const OptMatrix& guess) { guessMat_ = guess.block(0,0,rows_,cols_); guessValueInitialized_ = true; }
+
+ private:
+ //! Some helper functions to construct the problem
+ int getRowBndInd(int row) const;
+ int getColBndInd(int col) const;
+ int getBlockLen(int rini, int cini, int rend, int cend) const { return this->id(rend, cend) - this->id(rini, cini) + 1; }
+
+ private:
+ char type_;
+ bool guessValueInitialized_;
+ int indexPosition_, rows_, cols_;
+ OptMatrix lBndMat_, uBndMat_, guessMat_;
+ };
+}
diff --git a/ExternalSource/Optimizer/ConicSolver/include/solver/interface/Solver.hpp b/ExternalSource/Optimizer/ConicSolver/include/solver/interface/Solver.hpp
new file mode 100644
index 00000000..a4066c37
--- /dev/null
+++ b/ExternalSource/Optimizer/ConicSolver/include/solver/interface/Solver.hpp
@@ -0,0 +1,25 @@
+/*
+ * Copyright [2017] Max Planck Society. All rights reserved.
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see .
+ */
+
+#pragma once
+
+#include
+#include
+#include
+#include
+#include
+#include
diff --git a/ExternalSource/Optimizer/ConicSolver/include/solver/interface/SolverParams.hpp b/ExternalSource/Optimizer/ConicSolver/include/solver/interface/SolverParams.hpp
new file mode 100644
index 00000000..5193ba36
--- /dev/null
+++ b/ExternalSource/Optimizer/ConicSolver/include/solver/interface/SolverParams.hpp
@@ -0,0 +1,105 @@
+/*
+ * Copyright [2017] Max Planck Society. All rights reserved.
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see .
+ */
+
+#pragma once
+
+namespace solver {
+
+ /*! Available integer variables used by the optimizer */
+ enum SolverIntParam {
+ // Equilibration parameters
+ SolverIntParam_EquilibrationIters,
+
+ // Linear System parameters
+ SolverIntParam_NumIterRefinementsLinSolve,
+
+ // Model parameters
+ SolverIntParam_MaxIters,
+ SolverIntParam_WarmStartIters,
+ SolverIntParam_SolverMaxIters,
+ SolverIntParam_NumberRefinementsTrustRegion,
+
+ // Variable parameters
+ SolverIntParam_ColNum,
+
+ // OptimizationInfo parameters
+ SolverIntParam_NumIter,
+ SolverIntParam_NumRefsLinSolve,
+ SolverIntParam_NumRefsLinSolveAffine,
+ SolverIntParam_NumRefsLinSolveCorrector,
+ };
+
+ /*! Available boolean variables used by the optimizer */
+ enum SolverBoolParam {
+ SolverBoolParam_Verbose,
+ };
+
+ /*! Available double variables used by the optimizer */
+ enum SolverDoubleParam {
+ // Convergence tolerances
+ SolverDoubleParam_FeasibilityTol,
+ SolverDoubleParam_DualityGapAbsTol,
+ SolverDoubleParam_DualityGapRelTol,
+ SolverDoubleParam_FeasibilityTolInacc,
+ SolverDoubleParam_DualityGapAbsTolInacc,
+ SolverDoubleParam_DualityGapRelTolInacc,
+
+ // Linear System parameters
+ SolverDoubleParam_LinearSystemAccuracy,
+ SolverDoubleParam_ErrorReductionFactor,
+ SolverDoubleParam_StaticRegularization,
+ SolverDoubleParam_DynamicRegularization,
+ SolverDoubleParam_DynamicRegularizationThresh,
+
+ // Algorithm parameters
+ SolverDoubleParam_SafeGuard,
+ SolverDoubleParam_MinimumStepLength,
+ SolverDoubleParam_MaximumStepLength,
+ SolverDoubleParam_StepLengthScaling,
+ SolverDoubleParam_MinimumCenteringStep,
+ SolverDoubleParam_MaximumCenteringStep,
+
+ // Model parameters
+ SolverDoubleParam_TrustRegionThreshold,
+ SolverDoubleParam_SoftConstraintWeight,
+
+ // Variable parameters
+ SolverDoubleParam_X,
+ SolverDoubleParam_LB,
+ SolverDoubleParam_UB,
+ SolverDoubleParam_Guess,
+
+ // OptimizationInfo parameters
+ SolverDoubleParam_Tau,
+ SolverDoubleParam_Kappa,
+ SolverDoubleParam_DualCost,
+ SolverDoubleParam_PrimalCost,
+ SolverDoubleParam_DualityGap,
+ SolverDoubleParam_KappaOverTau,
+ SolverDoubleParam_DualResidual,
+ SolverDoubleParam_MeritFunction,
+ SolverDoubleParam_PrimalResidual,
+ SolverDoubleParam_DualInfeasibility,
+ SolverDoubleParam_RelativeDualityGap,
+ SolverDoubleParam_PrimalInfeasibility,
+
+ SolverDoubleParam_StepLength,
+ SolverDoubleParam_AffineStepLength,
+ SolverDoubleParam_CorrectionStepLength,
+ };
+
+}
diff --git a/ExternalSource/Optimizer/ConicSolver/include/solver/interface/SolverSetting.hpp b/ExternalSource/Optimizer/ConicSolver/include/solver/interface/SolverSetting.hpp
new file mode 100644
index 00000000..f86e24b9
--- /dev/null
+++ b/ExternalSource/Optimizer/ConicSolver/include/solver/interface/SolverSetting.hpp
@@ -0,0 +1,95 @@
+/*
+ *
+ * ECOS - Embedded Conic Solver
+ * Copyright (C) [2012-2015] A. Domahidi [domahidi@embotech.com],
+ * Automatic Control Lab, ETH Zurich & embotech GmbH, Zurich, Switzerland.
+ *
+ * Copyright [2017] Max Planck Society. All rights reserved.
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see .
+ */
+
+#pragma once
+
+#include
+#include
+#include
+
+namespace solver {
+
+ enum class ExitCode {
+ Optimal, /*! Problem solved to optimality */
+ OptimalInacc, /*! Problem solved to optimality (Inaccurate) */
+ PrimalInf, /*! Found certificate of primal infeasibility */
+ PrimalInfInacc, /*! Found certificate of primal infeasibility (Inaccurate) */
+ DualInf, /*! Found certificate of dual infeasibility */
+ DualInfInacc, /*! Found certificate of dual infeasibility (Inaccurate) */
+ ReachMaxIters, /*! Algorithm reached maximum number of iterations */
+ NotConverged, /*! Algorithm has not converged yet */
+ Indeterminate, /*! Nothing can be said about the solution */
+ PrSearchDirection, /*! Not reliable search direction */
+ PrSlacksLeaveCone, /*! Slack variables lie outside convex cone */
+ PrProjection, /*! Failed in the projection of cone or linear system */
+ };
+
+ enum class ConeStatus { Inside, Outside };
+ enum class FactStatus { Optimal, Failure };
+ enum class PrecisionConvergence { Full, Reduced };
+ enum class QuadConstrApprox { None, TrustRegion, SoftConstraint };
+
+ /**
+ * Class that provides access to all environment variables required by the solver
+ */
+ class SolverSetting
+ {
+ public:
+ SolverSetting(){}
+ ~SolverSetting(){}
+
+ void initialize(const std::string cfg_file, const std::string stgs_vars_yaml = "solver_variables");
+
+ int get(SolverIntParam param) const;
+ bool get(SolverBoolParam param) const;
+ double get(SolverDoubleParam param) const;
+
+ void set(SolverIntParam param, int value);
+ void set(SolverBoolParam param, bool value);
+ void set(SolverDoubleParam param, double value);
+
+ static constexpr double nan = ((double)0x7ff8000000000000);
+ static constexpr double inf = ((double)std::numeric_limits::infinity());
+
+ private:
+ // Convergence tolerances
+ double feasibility_tolerance_, absolute_suboptimality_gap_, relative_suboptimality_gap_,
+ feasibility_tolerance_inaccurate_, absolute_suboptimality_gap_inaccurate_, relative_suboptimality_gap_inaccurate_;
+
+ // Equilibration parameters
+ int equil_iterations_;
+
+ // Linear System parameters
+ int num_iter_ref_lin_solve_;
+ double dyn_reg_thresh_, lin_sys_accuracy_, err_reduction_factor_, static_regularization_, dynamic_regularization_;
+
+ // Algorithm parameters
+ double safeguard_, min_step_length_, max_step_length_, min_centering_step_, max_centering_step_, step_length_scaling_;
+
+ // Model parameters
+ bool verbose_;
+ double trust_region_threshold_, soft_constraint_weight_;
+ int max_iters_, num_itrefs_trustregion_, ipsolver_warm_iters_, ipsolver_max_iters_;
+
+ };
+
+}
diff --git a/ExternalSource/Optimizer/ConicSolver/include/solver/interface/Var.hpp b/ExternalSource/Optimizer/ConicSolver/include/solver/interface/Var.hpp
new file mode 100644
index 00000000..f0f88e60
--- /dev/null
+++ b/ExternalSource/Optimizer/ConicSolver/include/solver/interface/Var.hpp
@@ -0,0 +1,58 @@
+/*
+ * Copyright [2017] Max Planck Society. All rights reserved.
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see .
+ */
+
+#pragma once
+
+#include
+#include
+
+namespace solver {
+
+ enum class VarType {Continuous};
+
+ struct VarStorage
+ {
+ int col_no_;
+ VarType type_;
+ double lb_, ub_, value_, guess_;
+ };
+
+ /**
+ * Helper class to define an optimization variable, used in the
+ * construction of linear and quadratic expressions.
+ */
+ class Var
+ {
+ public:
+ Var() : var_storage_(nullptr) {};
+
+ int get(SolverIntParam param) const;
+ double get(SolverDoubleParam param) const;
+ void set(SolverIntParam param, int value);
+ void set(SolverDoubleParam param, double value);
+
+ friend class Model;
+ friend class LinExpr;
+
+ private:
+ Var(int col_no, const VarType& type, double lb, double ub, double guess=0.0);
+
+ private:
+ std::shared_ptr var_storage_;
+ };
+
+}
diff --git a/ExternalSource/Optimizer/ConicSolver/include/solver/optimizer/EqRoutine.hpp b/ExternalSource/Optimizer/ConicSolver/include/solver/optimizer/EqRoutine.hpp
new file mode 100644
index 00000000..abb9bf74
--- /dev/null
+++ b/ExternalSource/Optimizer/ConicSolver/include/solver/optimizer/EqRoutine.hpp
@@ -0,0 +1,58 @@
+/*
+ *
+ * ECOS - Embedded Conic Solver
+ * Copyright (C) [2012-2015] A. Domahidi [domahidi@embotech.com],
+ * Automatic Control Lab, ETH Zurich & embotech GmbH, Zurich, Switzerland.
+ *
+ * Copyright [2017] Max Planck Society. All rights reserved.
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see .
+ */
+
+#pragma once
+
+#include
+#include
+#include
+#include
+
+namespace solver {
+
+ /*! Equilibration routine to improve condition number of
+ * matrices involved in the optimization problem. The method
+ * provided by default is Ruiz equilibration.
+ */
+ class EqRoutine
+ {
+ public:
+ EqRoutine(){}
+ ~EqRoutine(){}
+
+ void setEquilibration(const Cone& cone, const SolverSetting& stgs, SolverStorage& stg);
+ void unsetEquilibration(SolverStorage& stg);
+ void scaleVariables(OptimizationVector& opt);
+
+ private:
+ void ruizEquilibration(SolverStorage& stg);
+ void maxRowsCols(double *row_vec, double *col_vec, const Eigen::SparseMatrix& mat);
+ void equilibrateRowsCols(const double *row_vec, const double *col_vec, Eigen::SparseMatrix& mat);
+ void unequilibrateRowsCols(const double *row_vec, const double *col_vec, Eigen::SparseMatrix& mat);
+
+ private:
+ Vector equil_vec_;
+ std::shared_ptr cone_;
+ std::shared_ptr stgs_;
+ };
+
+}
diff --git a/ExternalSource/Optimizer/ConicSolver/include/solver/optimizer/IPSolver.hpp b/ExternalSource/Optimizer/ConicSolver/include/solver/optimizer/IPSolver.hpp
new file mode 100644
index 00000000..1a78957f
--- /dev/null
+++ b/ExternalSource/Optimizer/ConicSolver/include/solver/optimizer/IPSolver.hpp
@@ -0,0 +1,98 @@
+/*
+ *
+ * ECOS - Embedded Conic Solver
+ * Copyright (C) [2012-2015] A. Domahidi [domahidi@embotech.com],
+ * Automatic Control Lab, ETH Zurich & embotech GmbH, Zurich, Switzerland.
+ *
+ * Copyright [2017] Max Planck Society. All rights reserved.
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see .
+ */
+
+#pragma once
+
+#include
+#include
+#include
+#include
+#include
+
+namespace solver {
+
+ /**
+ * Main class that implements an Interior Point Solver for Second-Order Cones.
+ * Details can be found in the paper: Domahidi, A. and Chu, E. and Boyd, S.,
+ * ECOS: An SOCP solver for embedded systems, ECC 2013, pages 3071-3076
+ */
+ class InteriorPointSolver
+ {
+ public:
+ InteriorPointSolver(){}
+ ~InteriorPointSolver() {}
+
+ void internalInitialization();
+ void initialize(SolverStorage& stg, Cone& cone, SolverSetting& stgs);
+ ExitCode optimize();
+
+ Cone& getCone() { return *cone_; }
+ SolverStorage& getStg() { return *stg_; }
+ SolverSetting& getStgs() { return *stgs_; }
+ EqRoutine& getEqRoutine() { return equil_; }
+ OptimizationInfo& getInfo() { return info_; }
+ InfoPrinter& getPrinter() { return printer_; }
+ LinSolver& getLinSolver() { return linsolver_; }
+ OptimizationInfo& getBestInfo() { return best_info_; }
+
+ const Cone& getCone() const { return *cone_; }
+ const SolverStorage& getStg() const { return *stg_; }
+ const SolverSetting& getStgs() const { return *stgs_; }
+ const EqRoutine& getEqRoutine() const { return equil_; }
+ const OptimizationInfo& getInfo() const { return info_; }
+ const InfoPrinter& getPrinter() const { return printer_; }
+ const LinSolver& getLinSolver() const { return linsolver_; }
+ const OptimizationInfo& getBestInfo() const { return best_info_; }
+
+ OptimizationVector& optsol() { return opt_; }
+ const OptimizationVector& optsol() const { return opt_; }
+
+ private:
+ void rhsAffineStep();
+ void saveIterateAsBest();
+ void restoreBestIterate();
+ void rhsCenteringPredictorStep();
+ ExitCode convergenceCheck(const PrecisionConvergence& mode);
+
+ void computeResiduals();
+ void updateStatistics();
+ ExitCode initializeVariables();
+ double lineSearch(const Eigen::Ref& dsvec, const Eigen::Ref& dzvec, double tau, double dtau, double kappa, double dkappa);
+
+ private:
+ Vector res_;
+ ExitCode exitcode_;
+ ExtendedVector RHS1_, RHS2_;
+ OptimizationVector opt_, best_opt_, dopt1_, dopt2_;
+ ConicVector lambda_, rho_, sigma_, lbar_, ds_affine_by_W_, W_times_dz_affine_, ds_combined_, dz_combined_;
+ double dk_combined_, dt_affine_, dk_affine_, inires_x_, inires_y_, inires_z_, dt_denom_,
+ residual_t_, residual_x_, residual_y_, residual_z_, cx_, by_, hz_, prev_pres_;
+
+ Cone* cone_;
+ EqRoutine equil_;
+ SolverStorage* stg_;
+ SolverSetting* stgs_;
+ LinSolver linsolver_;
+ InfoPrinter printer_;
+ OptimizationInfo info_, best_info_;
+ };
+}
diff --git a/ExternalSource/Optimizer/ConicSolver/include/solver/optimizer/InfoPrinter.hpp b/ExternalSource/Optimizer/ConicSolver/include/solver/optimizer/InfoPrinter.hpp
new file mode 100644
index 00000000..074b2ffc
--- /dev/null
+++ b/ExternalSource/Optimizer/ConicSolver/include/solver/optimizer/InfoPrinter.hpp
@@ -0,0 +1,82 @@
+/*
+ *
+ * ECOS - Embedded Conic Solver
+ * Copyright (C) [2012-2015] A. Domahidi [domahidi@embotech.com],
+ * Automatic Control Lab, ETH Zurich & embotech GmbH, Zurich, Switzerland.
+ *
+ * Copyright [2017] Max Planck Society. All rights reserved.
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see .
+ */
+
+#pragma once
+
+#include
+#include
+
+namespace solver {
+
+ enum class Msg {
+ MatrixFactorization, /*! Problem in matrix factorization */
+ SearchDirection, /*! Unreliable search direction */
+ NumericalProblem, /*! Numerical problems */
+ LineSearchStagnation, /*! Line search step too small */
+ VariablesLeavingCone, /*! Slacks or multipliers leaving cone */
+ MaxItersReached, /*! Maximum number of iterations reached */
+ OptimalityReached, /*! Problem solved to optimality */
+ PrimalInfeasibility, /*! Certificate of primal infeasibility found */
+ DualInfeasibility, /*! Certificate of dual infeasibility found */
+ OptimizationProgress, /*! Progress of current optimization iterate */
+ };
+
+ /**
+ * Helper class that contains information about the status of the optimization problem
+ */
+ class OptimizationInfo
+ {
+ public:
+ OptimizationInfo() : iteration_(-1){}
+ ~OptimizationInfo(){}
+
+ bool isBetterThan(const OptimizationInfo& info) const;
+ OptimizationInfo& operator=(const OptimizationInfo& other);
+
+ int& get(const SolverIntParam& param);
+ double& get(const SolverDoubleParam& param);
+ PrecisionConvergence& mode() { return mode_; }
+
+ const int& get(const SolverIntParam& param) const;
+ const double& get(const SolverDoubleParam& param) const;
+ const PrecisionConvergence& mode() const { return mode_; }
+
+ private:
+ PrecisionConvergence mode_;
+ int iteration_, linear_solve_refinements_, affine_linear_solve_refinements_, correction_linear_solve_refinements_;
+ double primal_cost_, dual_cost_, primal_residual_, dual_residual_, primal_infeasibility_, dual_infeasibility_, tau_, kappa_,
+ kappa_over_tau_, merit_function_, duality_gap_, relative_duality_gap_, correction_step_length_, affine_step_length, step_length_;
+ };
+
+ class InfoPrinter
+ {
+ public:
+ InfoPrinter(){}
+ ~InfoPrinter(){}
+
+ void initialize(const SolverSetting& stgs) { stgs_ = std::make_shared(stgs); }
+ void display(const Msg& msg, const OptimizationInfo& info);
+
+ private:
+ std::shared_ptr stgs_;
+ };
+}
diff --git a/ExternalSource/Optimizer/ConicSolver/include/solver/optimizer/LinSolver.hpp b/ExternalSource/Optimizer/ConicSolver/include/solver/optimizer/LinSolver.hpp
new file mode 100644
index 00000000..d428e430
--- /dev/null
+++ b/ExternalSource/Optimizer/ConicSolver/include/solver/optimizer/LinSolver.hpp
@@ -0,0 +1,88 @@
+/*
+ *
+ * ECOS - Embedded Conic Solver
+ * Copyright (C) [2012-2015] A. Domahidi [domahidi@embotech.com],
+ * Automatic Control Lab, ETH Zurich & embotech GmbH, Zurich, Switzerland.
+ *
+ * Copyright [2017] Max Planck Society. All rights reserved.
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see .
+ */
+
+#pragma once
+
+#include
+#include
+#include
+
+namespace solver {
+
+ /**
+ * Class that provides functionality for handling solution of linear systems
+ * in optimization problems. Performs symbolic and numeric factorization of
+ * kkt matrix, find permutation of kkt matrix to induce the best possible
+ * sparsity pattern, build and updates kkt matrix and its scalings as required.
+ */
+ class LinSolver
+ {
+ public:
+ LinSolver(){}
+ ~LinSolver(){}
+
+ void updateMatrix();
+ void initializeMatrix();
+ FactStatus numericFactorization();
+ void initialize(Cone& cone, SolverSetting& stgs, SolverStorage& stg);
+ int solve(const Eigen::Ref& permB, OptimizationVector& searchDir, bool is_initialization = false);
+ void matrixTransposeTimesVector(const Eigen::SparseMatrix& A,const Eigen::Ref& eig_x, Eigen::Ref eig_y, bool add = true, bool is_new = true);
+
+ // Some getter and setter methods
+ int perm(int id) { return perm_.indices()[id]; }
+ int invPerm(int id) { return invPerm_.indices()[id]; }
+ Eigen::PermutationMatrix& perm() { return perm_; }
+ Eigen::PermutationMatrix& invPerm() { return invPerm_; }
+ const Eigen::PermutationMatrix& perm() const { return perm_; }
+ const Eigen::PermutationMatrix& invPerm() const { return invPerm_; }
+
+ private:
+ SolverSetting& getStgs() { return *stgs_; }
+ Cone& getCone() { return *cone_; }
+ SolverStorage& getStg() { return *stg_; }
+ linalg::SparseCholesky& getCholesky() { return chol_; }
+
+ const Cone& getCone() const { return *cone_; }
+ const SolverSetting& getStgs() const { return *stgs_; }
+ const SolverStorage& getStg() const { return *stg_; }
+ const linalg::SparseCholesky& getCholesky() const { return chol_; }
+
+ void buildProblem();
+ void findPermutation();
+ void resizeProblemData();
+ void symbolicFactorization();
+
+ private:
+ Cone* cone_;
+ SolverStorage* stg_;
+ SolverSetting* stgs_;
+
+ ConicVector Gdx_;
+ linalg::SparseCholesky chol_;
+ double static_regularization_;
+ ExtendedVector sign_, permSign_;
+ Eigen::VectorXd permX_, Pe_, permdX_;
+ Eigen::SparseMatrix kkt_, permKkt_;
+ Eigen::PermutationMatrix perm_, invPerm_, permK_;
+ };
+
+}
diff --git a/ExternalSource/Optimizer/ConicSolver/include/solver/optimizer/SparseCholesky.hpp b/ExternalSource/Optimizer/ConicSolver/include/solver/optimizer/SparseCholesky.hpp
new file mode 100644
index 00000000..125ef39e
--- /dev/null
+++ b/ExternalSource/Optimizer/ConicSolver/include/solver/optimizer/SparseCholesky.hpp
@@ -0,0 +1,75 @@
+/*
+ *
+ * LDL Copyright (c) 2005-2012 by Timothy A. Davis. http://www.suitesparse.com
+ *
+ * LDL License:
+ *
+ * Your use or distribution of LDL or any modified version of
+ * LDL implies that you agree to this License.
+ *
+ * This library is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU Lesser General Public
+ * License as published by the Free Software Foundation; either
+ * version 2.1 of the License, or (at your option) any later version.
+ *
+ * This library is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+ * Lesser General Public License for more details.
+ *
+ * You should have received a copy of the GNU Lesser General Public
+ * License along with this library; if not, write to the Free Software
+ * Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301
+ * USA
+ *
+ * Permission is hereby granted to use or copy this program under the
+ * terms of the GNU LGPL, provided that the Copyright, this License,
+ * and the Availability of the original version is retained on all copies.
+ * User documentation of any code that uses this code or any modified
+ * version of this code must cite the Copyright, this License, the
+ * Availability note, and "Used by permission." Permission to modify
+ * the code and to distribute modified code is granted, provided the
+ * Copyright, this License, and the Availability note are retained,
+ * and a notice that the code was modified is included.
+ *
+ * Availability:
+ *
+ * http://www.suitesparse.com
+ *
+ * Stripped down by Alexander Domahidi, 2012.
+ * Modified to c++ code by Max Planck Society, 2017.
+ *
+ */
+
+#pragma once
+
+#include
+#include
+#include
+
+namespace linalg {
+
+ /**
+ * Class to perform an LDL factorization of a matrix
+ */
+ class SparseCholesky
+ {
+ public:
+ SparseCholesky(){}
+ ~SparseCholesky(){}
+
+ void analyzePattern(const Eigen::SparseMatrix& mat, const solver::SolverSetting& stgs);
+ int factorize(const Eigen::SparseMatrix& mat, const Eigen::Ref& sign);
+ void solve(const Eigen::Ref& b, double* x);
+ Eigen::VectorXd& solve(const Eigen::VectorXd& b);
+
+ private:
+ int n_;
+ double eps_, delta_;
+ Eigen::VectorXd D_, Y_, X_;
+ Eigen::SparseMatrix L_;
+ std::shared_ptr stgs_;
+ Eigen::VectorXi Parent_, Pattern_, Flag_, Lnnz_;
+ };
+
+}
diff --git a/ExternalSource/Optimizer/ConicSolver/src/solver/interface/Cone.cpp b/ExternalSource/Optimizer/ConicSolver/src/solver/interface/Cone.cpp
new file mode 100644
index 00000000..2e03074b
--- /dev/null
+++ b/ExternalSource/Optimizer/ConicSolver/src/solver/interface/Cone.cpp
@@ -0,0 +1,464 @@
+/*
+ *
+ * ECOS - Embedded Conic Solver
+ * Copyright (C) [2012-2015] A. Domahidi [domahidi@embotech.com],
+ * Automatic Control Lab, ETH Zurich & embotech GmbH, Zurich, Switzerland.
+ *
+ * Copyright [2017] Max Planck Society. All rights reserved.
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see .
+ */
+
+#include
+
+namespace solver {
+
+ // Scaling Operator
+ ConicVector ScalingOperator::operator*(const Eigen::Ref& rhs) const
+ {
+ ConicVector conic_scaling; conic_scaling.initialize(*(this->cone_));
+ this->cone_->conicNTScaling(rhs, conic_scaling.z());
+ return conic_scaling;
+ }
+
+ // Nesterov Todd scaling class
+ NesterovToddScaling::NesterovToddScaling(int conesize)
+ {
+ wbar_.resize(conesize);
+ SOC_id_.resize(3*conesize+1);
+ }
+
+ void Cone::initialize(int nvars, int nleq, int nlineq, const Eigen::VectorXi& nsoc)
+ {
+ neq_ = nleq;
+ nvars_ = nvars;
+ ssoc_ = nsoc.sum();
+ nsoc_ = nsoc.size();
+ extssoc_ = ssoc_ + 2*nsoc_;
+ lpconestart_ = nvars + nleq;
+ soconestart_ = nvars + nleq + nlineq;
+ sizecone_ = nlineq + ssoc_;
+ extsizecone_ = sizecone_ + 2*nsoc_;
+ sizeconstraints_ = nleq + sizecone_;
+ sizeproblem_ = nvars + sizeconstraints_;
+ extsizeproblem_ = sizeproblem_ + 2*nsoc_;
+ this->setupLpcone(nlineq);
+ this->setupSocone(nsoc);
+ W_scaling_operator_.initialize(*this);
+ skbar_.resize(sizecone_);
+ zkbar_.resize(sizecone_);
+ }
+
+ void Cone::setupLpcone(int size)
+ {
+ nineq_ = size;
+ LP_id_.resize(size);
+ LP_scaling_.resize(size);
+ LP_sq_scaling_.resize(size);
+ }
+
+ // Cone class
+ void Cone::setupSocone(const Eigen::VectorXi& indices)
+ {
+ q_ = indices;
+ soc_.clear();
+ SOC_start_.resize(nsoc_);
+ opt_SOC_start_.resize(nsoc_);
+ ext_SOC_start_.resize(nsoc_);
+ for (int i=0; i eig_u(u+1,size-1);
+ return u[0]*u[0] - eig_u.squaredNorm();
+ }
+
+ inline double Cone::conicResidual(const double* u, const double* v, int size) const
+ {
+ double result = u[0]*v[0];
+ for (int i=1; i& u) const
+ {
+ return this->conicResidual(u.data(), u.size());
+ }
+
+ double Cone::conicResidual(const Eigen::Ref& u, const Eigen::Ref& v) const
+ {
+ return this->conicResidual(u.data(), v.data(), u.size());
+ }
+
+ double Cone::conicProduct(const Eigen::Ref uvec,
+ const Eigen::Ref vvec, Eigen::Ref wvec) const
+ {
+ double* w = wvec.data();
+ const double* u = uvec.data();
+ const double* v = vvec.data();
+
+ // Linear cone
+ for (int i=0; isizeLpc(); i++) { w[i] = u[i]*v[i]; }
+ double mu = wvec.head(this->sizeLpc()).cwiseAbs().sum();
+
+ for (int i=0; inumSoc(); i++) {
+ w[this->startSoc(i)] = 0.0;
+ for (int j=0; jsizeSoc(i); j++)
+ w[this->startSoc(i)] += u[this->startSoc(i)+j]*v[this->startSoc(i)+j];
+
+ for (int j=1; jsizeSoc(i); j++)
+ w[this->startSoc(i)+j] = u[this->startSoc(i)]*v[this->startSoc(i)+j] + v[this->startSoc(i)]*u[this->startSoc(i)+j];
+
+ mu += std::abs(w[this->startSoc(i)]);
+ }
+ return mu;
+ }
+
+ void Cone::conicDivision(const Eigen::Ref& uvec, const Eigen::Ref& wvec, Eigen::Ref vvec) const
+ {
+ double* v = vvec.data();
+ const double* u = uvec.data();
+ const double* w = wvec.data();
+
+ // Linear cone
+ for (int i=0; isizeLpc(); i++) { v[i] = this->safeDivision(w[i],u[i]); }
+
+ // Second-order cone
+ double rho, uwn, factor;
+ for (int i=0; inumSoc(); i++) {
+ rho = conicResidual(u+this->startSoc(i),this->sizeSoc(i));
+ uwn = 0; for (int j=1; jsizeSoc(i); j++) { uwn += u[this->startSoc(i)+j]*w[this->startSoc(i)+j]; }
+ factor = this->safeDivision(this->safeDivision(uwn,u[this->startSoc(i)])-w[this->startSoc(i)],rho);
+ v[this->startSoc(i)] = this->safeDivision(u[this->startSoc(i)]*w[this->startSoc(i)]-uwn,rho);
+ for (int j=1; jsizeSoc(i); j++)
+ v[this->startSoc(i)+j] = factor*u[this->startSoc(i)+j] + this->safeDivision(w[this->startSoc(i)+j], u[this->startSoc(i)]);
+ }
+ }
+
+ void Cone::conicProjection(Eigen::Ref s)
+ {
+ // Find maximum residual
+ double* s_ptr = s.data();
+ double max_residual = 0.0;
+ for (int i=0; isizeLpc(); i++) { if (s(i) < 0.0) { max_residual = std::max(max_residual,fabs(s(i))); } }
+ for (int i=0; inumSoc(); i++) {
+ double residual = 0.0;
+ for (int j=0; jsizeSoc(i)-1; j++) { residual += std::pow(s_ptr[this->startSoc(i)+1+j], 2.0); }
+ residual = std::max(std::sqrt(residual)-s_ptr[this->startSoc(i)],0.0);
+ if (residual > max_residual) { max_residual = residual; }
+ }
+
+ // Scale variables appropriately into the cone
+ s.head(this->sizeLpc()).array() += max_residual + 1.0;
+ for (int i=0; inumSoc(); i++)
+ s_ptr[this->startSoc(i)] += max_residual + 1.0;
+ }
+
+ void Cone::conicNTScaling(const Eigen::VectorXd& z, Eigen::Ref lambda) const
+ {
+ this->conicNTScaling(z.data(), lambda.data());
+ }
+
+ void Cone::conicNTScaling(const double* z, double* lambda) const
+ {
+ // Linear cone
+ for (int i=0; isizeLpc(); i++){ lambda[i] = this->scalingLpc(i) * z[i]; }
+
+ // Second order cone
+ for (int l=0; lnumSoc(); l++) {
+ int conesize = this->sizeSoc(l);
+ int conestart = this->startSoc(l);
+ Eigen::Map eig_z1(z+conestart+1, conesize-1);
+
+ double lzero = this->soc(l).scalingSoc().tail(conesize-1).dot(eig_z1);
+ double factor = z[conestart] + this->safeDivision(lzero,(1.0+this->soc(l).scalingSoc(0)));
+
+ lambda[conestart] = this->soc(l).eta()*(this->soc(l).scalingSoc(0)*z[conestart] + lzero);
+ for (int i=1; isizeSoc(l); i++)
+ lambda[conestart+i] = this->soc(l).eta()*(z[conestart+i] + factor*this->soc(l).scalingSoc(i));
+ }
+ }
+
+ void Cone::conicNTScaling2(const Eigen::VectorXd& xvec, Eigen::Ref yvec) const
+ {
+ double* y = yvec.data();
+ const double* x = xvec.data(), *scaling_soc;
+
+ // Linear cone
+ const double* sq_scaling_lpc = this->sqScalingLpc().data();
+ for (int i=0; isizeLpc(); i++) { y[i] += sq_scaling_lpc[i] * x[i]; }
+
+ // Second order cone
+ int conesize, conestart;
+ for (int l=0; lnumSoc(); l++) {
+ conesize = this->sizeSoc(l);
+ conestart = this->startSoc(l) + 2*l;
+ scaling_soc = this->soc(l).scalingSoc().data();
+
+ y[conestart] += this->soc(l).etaSquare()*(this->soc(l).d1()*x[conestart] + this->soc(l).u0()*x[conestart+conesize+1]);
+ double yaux1 = this->soc(l).v1()*x[conestart+conesize] + this->soc(l).u1()*x[conestart+conesize+1];
+ double yaux2 = 0.0;
+ for (int i=1; isoc(l).etaSquare()*(x[conestart+i] + yaux1*scaling_soc[i]);
+ yaux2 += scaling_soc[i]*x[conestart+i];
+ }
+
+ y[conestart+conesize ] += this->soc(l).etaSquare()*(this->soc(l).v1()*yaux2 + x[conestart+conesize]);
+ y[conestart+conesize+1] += this->soc(l).etaSquare()*(this->soc(l).u0()*x[conestart] + this->soc(l).u1()*yaux2 - x[conestart+conesize+1]);
+ }
+ }
+
+ void Cone::unpermuteSolution(const Eigen::PermutationMatrix& Perm,
+ const Eigen::VectorXd& Px, OptimizationVector& sd) const
+ {
+ double* dx = sd.x().data();
+ double* dy = sd.y().data();
+ double* dz = sd.z().data();
+ const int* perm = Perm.indices().data();
+
+ int k=this->soConeStart(), j=this->sizeLpc();
+ for (int i=0; inumVars(); i++) { dx[i] = Px[perm[i]]; }
+ for (int i=0; inumLeq(); i++) { dy[i] = Px[perm[i+this->numVars()]]; }
+ for (int i=0; isizeLpc(); i++) { dz[i] = Px[perm[i+this->lpConeStart()]]; }
+ for (int l=0; lnumSoc(); l++) {
+ for (int i=0; isizeSoc(l); i++)
+ dz[j++] = Px[perm[k++]];
+ k += 2;
+ }
+ }
+
+ void Cone::unpermuteSolution(const Eigen::PermutationMatrix& Perm,
+ const Eigen::VectorXd& Px, OptimizationVector& sd, Eigen::VectorXd& permdZ) const
+ {
+ double* dx = sd.x().data();
+ double* dy = sd.y().data();
+ double* dz = sd.z().data();
+ const int* perm = Perm.indices().data();
+
+ int k=this->soConeStart(), j=this->sizeLpc();
+ for (int i=0; inumVars(); i++) { dx[i] = Px[perm[i]]; }
+ for (int i=0; inumLeq(); i++) { dy[i] = Px[perm[i+this->numVars()]]; }
+ for (int i=0; isizeLpc(); i++) { dz[i] = Px[perm[i+this->lpConeStart()]]; }
+ for (int l=0; lnumSoc(); l++) {
+ for (int i=0; isizeSoc(l); i++)
+ dz[j++] = Px[perm[k++]];
+ k += 2;
+ }
+ for (int i=0; iextSizeCone(); i++) { permdZ[i] = Px[perm[i+this->lpConeStart()]]; }
+ }
+
+ ConeStatus Cone::updateNTScalings(const Eigen::VectorXd& svec, const Eigen::VectorXd& zvec, Eigen::VectorXd& lambda)
+ {
+ const double* s = svec.data();
+ const double* z = zvec.data();
+ double sresidual, zresidual, w0, wn, alpha, beta;
+
+ // Linear cone
+ for (int i=0; isizeLpc(); i++) {
+ this->sqScalingLpc(i) = this->safeDivision(s[i], z[i]);
+ this->scalingLpc(i) = std::sqrt(this->sqScalingLpc(i));
+ }
+
+ // Second order cone
+ for (int l=0; lnumSoc(); l++) {
+ int conesize = this->sizeSoc(l);
+ int conestart = this->startSoc(l);
+
+ // check residuals
+ sresidual = this->conicResidual(s+conestart, conesize);
+ zresidual = this->conicResidual(z+conestart, conesize);
+ if ( sresidual<=0 || zresidual<=0 ) { return ConeStatus::Outside; }
+
+ // normalize variables
+ Eigen::Map skbar(&skbar_[conestart], conesize);
+ Eigen::Map zkbar(&zkbar_[conestart], conesize);
+ double sresidual_inv = this->safeDivision(1.0, std::sqrt(sresidual));
+ double zresidual_inv = this->safeDivision(1.0, std::sqrt(zresidual));
+ for (int i=0; isoc(l).etaSquare() = this->safeDivision(std::sqrt(sresidual), std::sqrt(zresidual));
+ this->soc(l).eta() = sqrt(this->soc(l).etaSquare());
+
+ // computing Nesterov-Todd scaling point
+ double normalizer = this->safeDivision(0.5,std::sqrt(0.5 + 0.5*skbar.dot(zkbar)));
+ this->soc(l).scalingSoc(0) = normalizer*(skbar[0] + zkbar[0]);
+ this->soc(l).w() = 0.0;
+ for (int i=1; isoc(l).scalingSoc(i) = normalizer*(skbar[i] - zkbar[i]);
+ this->soc(l).w() += std::pow(this->soc(l).scalingSoc(i), 2.0);
+ }
+
+ // computing variables for KKT matrix update
+ w0 = this->soc(l).scalingSoc(0);
+ wn = this->soc(l).scalingSoc().tail(conesize-1).squaredNorm();
+ alpha = 1.0 + w0 + this->safeDivision(wn, 1.0 + w0);
+ beta = 1.0 + this->safeDivision(2.0, 1.0 + w0) + this->safeDivision(wn,std::pow(1.0 + w0, 2.0));
+ this->soc(l).d1() = std::max(0.5*(w0*w0 + wn*(1.0 - this->safeDivision(alpha*alpha,1.0 + wn*beta) )), 0.0);
+ this->soc(l).u0() = std::sqrt(w0*w0+wn-this->soc(l).d1());
+ this->soc(l).u1() = this->safeDivision(alpha,this->soc(l).u0());
+ this->soc(l).v1() = std::pow(this->soc(l).u1(),2.0) - beta;
+ if (this->soc(l).v1() <= 0) { return ConeStatus::Outside; }
+ this->soc(l).v1() = std::sqrt(this->soc(l).v1());
+ }
+ this->conicNTScaling(zvec, lambda);
+
+ return ConeStatus::Inside;
+ }
+
+ // Vector class
+ void Vector::initialize(const Cone& cone)
+ {
+ cone_ = &cone;
+ this->resize(cone_->sizeProb());
+ this->setZero();
+ }
+
+ // ConicVector class
+ void ConicVector::initialize(const Cone& cone)
+ {
+ cone_ = &cone;
+ this->resize(cone_->sizeCone());
+ this->setZero();
+ }
+
+ ConicVector ConicVector::operator/(const ConicVector& rhs) const
+ {
+ ConicVector conic_division; conic_division.initialize(*(this->cone_));
+ this->cone_->conicDivision(this->z(), rhs.z(), conic_division.z());
+ return conic_division;
+ }
+
+ ConicVector ConicVector::operator*(const ConicVector& rhs) const
+ {
+ ConicVector conic_product; conic_product.initialize(*(this->cone_));
+ this->cone_->conicProduct(this->z(), rhs.z(), conic_product.z());
+ return conic_product;
+ }
+
+ ConicVector ConicVector::operator+(const ConicVector& rhs) const
+ {
+ ConicVector conic_sum; conic_sum.initialize(*(this->cone_));
+ conic_sum.z() = this->z() + rhs.z();
+ return conic_sum;
+ }
+
+ ConicVector ConicVector::operator+(const double& rhs) const
+ {
+ ConicVector conic_sum; conic_sum.initialize(*(this->cone_));
+ conic_sum.z() = this->z();
+ conic_sum.zLpc().array() += rhs;
+ for (int i=0; icone_->numSoc(); i++)
+ conic_sum.zSoc(i)[0] += rhs;
+ return conic_sum;
+ }
+
+ ConicVector ConicVector::operator-(const double& rhs) const
+ {
+ ConicVector conic_diff; conic_diff.initialize(*(this->cone_));
+ conic_diff.z() = this->z();
+ conic_diff.zLpc().array() -= rhs;
+ for (int i=0; i