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; icone_->numSoc(); i++) + conic_diff.zSoc(i)[0] -= rhs; + return conic_diff; + } + + ConicVector& ConicVector::operator+=(const double& rhs) + { + this->zLpc().array() += rhs; + for (int i=0; icone_->numSoc(); i++) + this->zSoc(i)[0] += rhs; + return *this; + } + + ConicVector ConicVector::operator-() const + { + ConicVector conic_vec; conic_vec.initialize(*(this->cone_)); + conic_vec.z() = -this->z(); + return conic_vec; + } + + // ExtendedVector class + void ExtendedVector::initialize(const Cone& cone) + { + cone_ = &cone; + this->resize(cone_->extSizeProb()); + this->setZero(); + } + + void ExtendedVector::zRtoE(const Eigen::Ref& rvec) + { + this->zLpc() = rvec.head(cone_->sizeLpc()); + for (int i=0; inumSoc(); i++) { + this->zSoc(i).tail(2).setZero(); + this->zSoc(i).head(cone_->sizeSoc(i)) = rvec.segment(cone_->startSoc(i),cone_->sizeSoc(i)); + } + } + + // OptimizationVector class + void OptimizationVector::initialize(const Cone& cone) + { + cone_ = &cone; + this->resize(cone_->sizeProb()+cone_->sizeLpc()+cone_->sizeSoc()+2); + this->setZero(); + } + + OptimizationVector OptimizationVector::operator+(const OptimizationVector& rhs) const + { + OptimizationVector result = *this; + result += rhs; + return result; + } + + // SolverStorage + void SolverStorage::initialize(Cone& cone, SolverSetting& stgs) + { + stgs_ = &stgs; + cone_ = &cone; + cbh_.initialize(cone); + u_opt_.initialize(cone); + v_opt_.initialize(cone); + u_t_opt_.initialize(cone); + cbh_copy_.initialize(cone); + u_prev_opt_.initialize(cone); + this->A().resize(cone_->numLeq(), cone_->numVars()); + this->G().resize(cone_->sizeLpc() + cone_->sizeSoc(), cone_->numVars()); + } + + void SolverStorage::addCoeff(const Eigen::Triplet& coeff, bool flag_eq) + { + if (flag_eq == true) { this->Acoeffs().push_back(coeff); } + else { this->Gcoeffs().push_back(coeff); } + } + + void SolverStorage::initializeMatrices() + { + this->A().setFromTriplets(Acoeffs_.begin(), Acoeffs_.end()); + if (!this->A().isCompressed()) { this->A().makeCompressed(); } + + this->G().setFromTriplets(Gcoeffs_.begin(), Gcoeffs_.end()); + if (!this->G().isCompressed()) { this->G().makeCompressed(); } + } + +} diff --git a/ExternalSource/Optimizer/ConicSolver/src/solver/interface/Exprs.cpp b/ExternalSource/Optimizer/ConicSolver/src/solver/interface/Exprs.cpp new file mode 100644 index 00000000..7363191f --- /dev/null +++ b/ExternalSource/Optimizer/ConicSolver/src/solver/interface/Exprs.cpp @@ -0,0 +1,94 @@ +/* + * 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 +#include +#include +#include + +namespace solver { + + // Class for Linear Expressions + double LinExpr::getValue() const { + double value = constant_; + for (unsigned int i = 0; i < vars_.size(); i++) + value += coeffs_[i]*vars_[i].get(SolverDoubleParam_X); + return value; + } + + bool LinExpr::isClean(const LinExpr& rhs) { + for (int i=1; icol_no_ == rhs.vars_[j].var_storage_->col_no_) + return false; + } + if (rhs.coeffs_[i] == 0.0) + return false; + } + return true; + } + + LinExpr LinExpr::clean(const LinExpr& rhs) { + LinExpr result; + result.constant_ = rhs.constant_; + + for (int i=0; icol_no_ == result.vars_[j].var_storage_->col_no_) { + result.coeffs_[j] += (rhs.coeffs_[i]); + set = true; + } + } + if (!set && rhs.coeffs_[i] != 0.0) { + result.coeffs_.push_back(rhs.coeffs_[i]); + result.vars_.push_back(rhs.vars_[i]); + } + } + return result; + } + + LinExpr LinExpr::operator*(double factor) const { + if (factor != 0.0) { + LinExpr result = *this; + result.getConstant() *= factor; + for (int i=0; i<(int)result.size(); i++) + result.coeffs_[i] *= factor; + return LinExpr::clean(result); + } + return LinExpr(); + } + + LinExpr& LinExpr::operator+=(const LinExpr& rhs) { + this->getConstant() += rhs.getConstant(); + for (int i=0; i<(int)rhs.size(); i++) { + this->vars_.push_back(rhs.vars_[i]); + this->coeffs_.push_back(rhs.coeffs_[i]); + } + *this = LinExpr::clean(*this); + return *this; + } + + // Class for Disciplined Convex Quadratic Expressions + double DCPQuadExpr::getValue() const { + double value = 0.0; + for (unsigned int i=0; i. + */ + +#include +#include +#include + +namespace solver { + + void Model::configSetting(const std::string cfg_file, const std::string stgs_vars_yaml) + { + this->getStgs().initialize(cfg_file, stgs_vars_yaml); + } + + void Model::clean() + { + vars_.clear(); + leqcons_.clear(); + soccons_.clear(); + lineqcons_.clear(); + qineqcons_.clear(); + objective_.clear(); + numTrustRegions_ = 0; + numSoftConstraints_ = 0; + } + + Var Model::addVar(const VarType& type, double lb, double ub, double guess) + { + if (lb > ub) { throw std::runtime_error("Invalid assertion LB <= UB"); } + if (guess < lb) { guess = lb; if (this->getStgs().get(SolverBoolParam_Verbose)) { std::cerr << "Warning: Guess < LB, Setting: Guess = LB" << std::endl; } } + else if (guess > ub) { guess = ub; if (this->getStgs().get(SolverBoolParam_Verbose)) { std::cerr << "Warning: Guess > UB, Setting: Guess = UB" << std::endl; } } + + Var var = Var(vars_.size(), type, lb, ub, guess); + vars_.push_back( std::make_shared( var ) ); + return var; + } + + // Linear Constraint: left_hand_side [< = >] right_hand_side + void Model::addLinConstr(const LinExpr& lhs, const std::string sense, const LinExpr& rhs) + { + if (sense == "=") { leqcons_.push_back(lhs-rhs); } + else if (sense == "<") { lineqcons_.push_back(lhs-rhs); } + else if (sense == ">") { lineqcons_.push_back(rhs-lhs); } + else { throw std::runtime_error("Invalid sense on Linear Constraint"); } + } + + // Disciplined Convex Quadratic Constraint: Sum coeffs[i]* (DCP.qexpr[i])^2 + (DCP.lexpr - lexpr) [< =] 0.0 + void Model::addQuaConstr(const DCPQuadExpr& qexpr, const std::string sense, const LinExpr& lexpr, const QuadConstrApprox& qapprox) + { + DCPQuadExpr qstg = qexpr; + qstg.lexpr() = qexpr.lexpr() - lexpr; + switch (qapprox) { + case QuadConstrApprox::TrustRegion: { numTrustRegions_ += 1; qstg.trustRegion() = true; break; } + case QuadConstrApprox::SoftConstraint: { numSoftConstraints_ +=1; qstg.softConstraint() = true; break; } + default: { break; } + } + for (unsigned int i=0; iaddVar(VarType::Continuous, 0.0, 1.0, 0.5)); + if (qapprox == QuadConstrApprox::SoftConstraint) { qstg.extraVars().push_back(this->addVar(VarType::Continuous, 0.0, 1.0, 0.5)); } + + if (sense == "<") { qineqcons_.push_back(qstg); } + else { throw std::runtime_error("Invalid sense on Quadratic Constraint"); } + } + + // Second-Order Cone Constraint: SQRT( Sum (DCP.qexpr[i])^2 ) + (DCP.lexpr - lexpr) [< =] 0.0 + void Model::addSocConstr(const DCPQuadExpr& qexpr, const std::string sense, const LinExpr& lexpr) + { + DCPQuadExpr qstg = qexpr; + qstg.trustRegion() = false; + qstg.lexpr() = qexpr.lexpr() - lexpr; + qstg.extraVars().push_back(this->addVar(VarType::Continuous, 0.0, 1.0, 0.5)); + + if (sense == "<") { soccons_.push_back(qstg); } + else { throw std::runtime_error("Invalid sense on Second-Order Cone Constraint"); } + } + + + // Quadratic Objective: min Sum DCP.coeffs[i]*(DCP.qexpr[i])^2 + (DCP.lexpr+lexpr) + void Model::setObjective(const DCPQuadExpr& qexpr, const LinExpr& lexpr) + { + DCPQuadExpr qstg; + objective_.qexpr() = qexpr.qexpr(); + objective_.coeffs() = qexpr.coeffs(); + objective_.lexpr() = qexpr.lexpr() + lexpr; + for (int i=0; i<(int)qexpr.coeffs().size(); i++) + objective_.extraVars().push_back(this->addVar(VarType::Continuous, 0.0, 1.0, 0.5)); + } + + // Translate problem to standard conic form + void Model::build_problem(int iter_id, bool warm_start) + { + // problem size + numTrustRegions_ = 0; + numSoftConstraints_ = 0; + int row_start = 0, row_offset = 0; + int mextra = objective_.extraVars().size() + soccons_.size(); + for (int id=0; id<(int)qineqcons_.size(); id++) { + mextra += qineqcons_[id].coeffs().size(); + if (qineqcons_[id].trustRegion()) { numTrustRegions_ += 1; } + if (qineqcons_[id].softConstraint()) { numSoftConstraints_ += 1; } + if (qineqcons_[id].softConstraint() && !warm_start) { mextra += 1; } + } + + int nvars = vars_.size(); + int nleq = leqcons_.size(); + int nlineq = lineqcons_.size() + qineqcons_.size() + + soccons_.size() + (warm_start == true ? 0 : numTrustRegions_); + Eigen::VectorXi q(mextra); q.setConstant(3); + for (int id=0; id<(int)soccons_.size(); id++) + q.tail(soccons_.size())[id] = soccons_[id].coeffs().size()+1; + + this->getCone().initialize(nvars, nleq, nlineq, q); + this->getStorage().initialize(cone_, stgs_); + this->getStorage().cleanCoeffs(); + + // Linear equality constraints + for (int row_id=0; row_id<(int)leqcons_.size(); row_id++) { + for (int var_id=0; var_id<(int)leqcons_[row_id].size(); var_id++) { + this->getStorage().addCoeff(Eigen::Triplet(row_start+row_id, + leqcons_[row_id].getVar(var_id).get(SolverIntParam_ColNum), + leqcons_[row_id].getCoeff(var_id)), true); + this->getStorage().b()[row_start+row_id] = -leqcons_[row_id].getConstant(); + } + } + + // Linear inequality constraints + for (int row_id=0; row_id<(int)lineqcons_.size(); row_id++) { + for (int var_id=0; var_id<(int)lineqcons_[row_id].size(); var_id++) { + this->getStorage().addCoeff(Eigen::Triplet(row_start+row_offset+row_id, + lineqcons_[row_id].getVar(var_id).get(SolverIntParam_ColNum), + lineqcons_[row_id].getCoeff(var_id))); + this->getStorage().h()[row_start+row_id] = -lineqcons_[row_id].getConstant(); + } + } + row_start += lineqcons_.size(); + + // Quadratic inequality constraints linear part + for (int row_id=0; row_id<(int)qineqcons_.size(); row_id++) { + for (int extra_var_id=0; extra_var_id<(int)qineqcons_[row_id].coeffs().size(); extra_var_id++) { + this->getStorage().addCoeff(Eigen::Triplet(row_start+row_offset+row_id, + qineqcons_[row_id].extraVars()[extra_var_id].get(SolverIntParam_ColNum), + qineqcons_[row_id].coeffs()[extra_var_id])); + } + for (int lvar_id=0; lvar_id<(int)qineqcons_[row_id].lexpr().size(); lvar_id++) { + this->getStorage().addCoeff(Eigen::Triplet(row_start+row_offset+row_id, + qineqcons_[row_id].lexpr().getVar(lvar_id).get(SolverIntParam_ColNum), + qineqcons_[row_id].lexpr().getCoeff(lvar_id))); + } + this->getStorage().h()[row_start+row_id] = -qineqcons_[row_id].lexpr().getConstant(); + } + row_start += qineqcons_.size(); + + // Second order cone constraints linear part + for (int row_id=0; row_id<(int)soccons_.size(); row_id++) { + int extra_var_id = 0; + this->getStorage().addCoeff(Eigen::Triplet(row_start+row_offset+row_id, + soccons_[row_id].extraVars()[extra_var_id].get(SolverIntParam_ColNum), + 1.0)); + for (int lvar_id=0; lvar_id<(int)soccons_[row_id].lexpr().size(); lvar_id++) { + this->getStorage().addCoeff(Eigen::Triplet(row_start+row_offset+row_id, + soccons_[row_id].lexpr().getVar(lvar_id).get(SolverIntParam_ColNum), + soccons_[row_id].lexpr().getCoeff(lvar_id))); + } + this->getStorage().h()[row_start+row_id] = -soccons_[row_id].lexpr().getConstant(); + } + row_start += soccons_.size(); + + // Objective linear part + for (int var_id=0; var_id<(int)objective_.lexpr().size(); var_id++) + this->getStorage().c()[objective_.lexpr().getVar(var_id).get(SolverIntParam_ColNum)] = objective_.lexpr().getCoeff(var_id); + + if (!warm_start) { + // Quadratic constraints with trust region + int row_count = 0; + for (int row_id=0; row_id<(int)qineqcons_.size(); row_id++) { + if (qineqcons_[row_id].trustRegion()) { + this->getStorage().h()[row_start+row_count] = 0.0; + for (int qvar_id=0; qvar_id<(int)qineqcons_[row_id].qexpr().size(); qvar_id++) { + for (int lvar_id=0; lvar_id<(int)qineqcons_[row_id].qexpr()[qvar_id].size(); lvar_id++) { + this->getStorage().addCoeff(Eigen::Triplet(row_start+row_offset+row_count, + qineqcons_[row_id].getVar(qvar_id, lvar_id).get(SolverIntParam_ColNum), + -2.0*qineqcons_[row_id].coeffs()[qvar_id]*qineqcons_[row_id].qexpr()[qvar_id].getValue()*qineqcons_[row_id].qexpr()[qvar_id].getCoeff(lvar_id))); + this->getStorage().h()[row_start+row_count] -= 2.0*qineqcons_[row_id].coeffs()[qvar_id]*qineqcons_[row_id].qexpr()[qvar_id].getValue()*qineqcons_[row_id].qexpr()[qvar_id].getCoeff(lvar_id)*qineqcons_[row_id].getVar(qvar_id, lvar_id).get(SolverDoubleParam_X); + } + } + + for (int lvar_id=0; lvar_id<(int)qineqcons_[row_id].lexpr().size(); lvar_id++) { + this->getStorage().addCoeff(Eigen::Triplet(row_start+row_offset+row_count, + qineqcons_[row_id].lexpr().getVar(lvar_id).get(SolverIntParam_ColNum), + -qineqcons_[row_id].lexpr().getCoeff(lvar_id))); + } + this->getStorage().h()[row_start+row_count] += qineqcons_[row_id].lexpr().getConstant() + qineqcons_[row_id].getValue() + std::pow(this->getStgs().get(SolverDoubleParam_TrustRegionThreshold), iter_id); + row_count += 1; + } + } + row_start += numTrustRegions_; + } + + // Objective quadratic part + for (int extra_var_id=0; extra_var_id<(int)objective_.extraVars().size(); extra_var_id++) { + this->getStorage().addCoeff(Eigen::Triplet(row_start+row_offset+3*extra_var_id+0, + objective_.extraVars()[extra_var_id].get(SolverIntParam_ColNum), + -1.0)); + this->getStorage().addCoeff(Eigen::Triplet(row_start+row_offset+3*extra_var_id+1, + objective_.extraVars()[extra_var_id].get(SolverIntParam_ColNum), + -1.0)); + for (int qvar_id=0; qvar_id<(int)objective_.qexpr()[extra_var_id].size(); qvar_id++) { + this->getStorage().addCoeff(Eigen::Triplet(row_start+row_offset+3*extra_var_id+2, + objective_.qexpr()[extra_var_id].getVar(qvar_id).get(SolverIntParam_ColNum), + -2.0*objective_.qexpr()[extra_var_id].getCoeff(qvar_id))); + } + this->getStorage().h()[row_start+3*extra_var_id+0] = 1.0; + this->getStorage().h()[row_start+3*extra_var_id+1] = -1.0; + this->getStorage().h()[row_start+3*extra_var_id+2] = 2.0*objective_.qexpr()[extra_var_id].getConstant(); + this->getStorage().c()[objective_.extraVars()[extra_var_id].get(SolverIntParam_ColNum)] = objective_.coeffs()[extra_var_id]; + } + row_start += 3*objective_.extraVars().size(); + + if (!warm_start) { + // Adding quadratic objective terms due to soft constraints + int row_count = 0; + for (int row_id=0; row_id<(int)qineqcons_.size(); row_id++) { + if (qineqcons_[row_id].softConstraint()) { + this->getStorage().h()[row_start+3*row_count+0] = 1.0; + this->getStorage().h()[row_start+3*row_count+1] = -1.0; + this->getStorage().h()[row_start+3*row_count+2] = 0.0; + this->getStorage().addCoeff(Eigen::Triplet(row_start+row_offset+3*row_count+0, + qineqcons_[row_id].extraVars().back().get(SolverIntParam_ColNum), + -1.0)); + this->getStorage().addCoeff(Eigen::Triplet(row_start+row_offset+3*row_count+1, + qineqcons_[row_id].extraVars().back().get(SolverIntParam_ColNum), + -1.0)); + for (int qvar_id=0; qvar_id<(int)qineqcons_[row_id].qexpr().size(); qvar_id++) { + for (int lvar_id=0; lvar_id<(int)qineqcons_[row_id].qexpr()[qvar_id].size(); lvar_id++) { + this->getStorage().addCoeff(Eigen::Triplet(row_start+row_offset+3*row_count+2, + qineqcons_[row_id].getVar(qvar_id, lvar_id).get(SolverIntParam_ColNum), + -4.0*qineqcons_[row_id].coeffs()[qvar_id]*qineqcons_[row_id].qexpr()[qvar_id].getValue()*qineqcons_[row_id].qexpr()[qvar_id].getCoeff(lvar_id))); + this->getStorage().h()[row_start+3*row_count+2] -= 2.0*qineqcons_[row_id].coeffs()[qvar_id]*qineqcons_[row_id].qexpr()[qvar_id].getValue()*qineqcons_[row_id].qexpr()[qvar_id].getCoeff(lvar_id)*qineqcons_[row_id].getVar(qvar_id, lvar_id).get(SolverDoubleParam_X); + } + } + for (int lvar_id=0; lvar_id<(int)qineqcons_[row_id].lexpr().size(); lvar_id++) { + this->getStorage().addCoeff(Eigen::Triplet(row_start+row_offset+3*row_count+2, + qineqcons_[row_id].lexpr().getVar(lvar_id).get(SolverIntParam_ColNum), + -2.0*qineqcons_[row_id].lexpr().getCoeff(lvar_id) )); + } + this->getStorage().h()[row_start+3*row_count+2] += qineqcons_[row_id].lexpr().getConstant() + qineqcons_[row_id].getValue(); + this->getStorage().h()[row_start+3*row_count+2] = 2.0*this->getStorage().h()[row_start+3*row_count+2]; + this->getStorage().c()[qineqcons_[row_id].extraVars().back().get(SolverIntParam_ColNum)] = this->getStgs().get(SolverDoubleParam_SoftConstraintWeight); + row_count += 1; + } + } + row_start += 3*numSoftConstraints_; + } + + // Quadratic inequality constraints quadratic part + for (int row_id=0; row_id<(int)qineqcons_.size(); row_id++) { + for (int extra_var_id=0; extra_var_id<(int)qineqcons_[row_id].coeffs().size(); extra_var_id++) { + this->getStorage().addCoeff(Eigen::Triplet(row_start+row_offset+3*extra_var_id+0, + qineqcons_[row_id].extraVars()[extra_var_id].get(SolverIntParam_ColNum), + -1.0)); + this->getStorage().addCoeff(Eigen::Triplet(row_start+row_offset+3*extra_var_id+1, + qineqcons_[row_id].extraVars()[extra_var_id].get(SolverIntParam_ColNum), + -1.0)); + for (int qvar_id=0; qvar_id<(int)qineqcons_[row_id].qexpr()[extra_var_id].size(); qvar_id++) { + this->getStorage().addCoeff(Eigen::Triplet(row_start+row_offset+3*extra_var_id+2, + qineqcons_[row_id].qexpr()[extra_var_id].getVar(qvar_id).get(SolverIntParam_ColNum), + -2.0*qineqcons_[row_id].qexpr()[extra_var_id].getCoeff(qvar_id))); + } + this->getStorage().h()[row_start+3*extra_var_id+0] = 1.0; + this->getStorage().h()[row_start+3*extra_var_id+1] = -1.0; + this->getStorage().h()[row_start+3*extra_var_id+2] = 2.0*qineqcons_[row_id].qexpr()[extra_var_id].getConstant(); + } + row_start += 3*qineqcons_[row_id].coeffs().size(); + } + + // Second order cone constraints quadratic part + for (int row_id=0; row_id<(int)soccons_.size(); row_id++) { + int size = soccons_[row_id].coeffs().size(); + this->getStorage().addCoeff(Eigen::Triplet(row_start+row_offset+0, + soccons_[row_id].extraVars()[0].get(SolverIntParam_ColNum), + -1.0)); + this->getStorage().h()[row_start+0] = 0.0; + + for (int qvar_id=0; qvar_id<(int)soccons_[row_id].qexpr().size(); qvar_id++) { + for (int lvar_id=0; lvar_id<(int)soccons_[row_id].qexpr()[qvar_id].size(); lvar_id++) { + this->getStorage().addCoeff(Eigen::Triplet(row_start+row_offset+qvar_id+1, + soccons_[row_id].qexpr()[qvar_id].getVar(lvar_id).get(SolverIntParam_ColNum), + -soccons_[row_id].qexpr()[qvar_id].getCoeff(lvar_id))); + } + this->getStorage().h()[row_start+qvar_id+1] = soccons_[row_id].qexpr()[qvar_id].getConstant(); + } + row_start += size+1; + } + + this->getStorage().initializeMatrices(); + ip_solver_.initialize(this->getStorage(), this->getCone(), this->getStgs()); + } + + ExitCode Model::solve_problem() + { + ExitCode exit_code = ExitCode::Indeterminate; + exit_code = ip_solver_.optimize(); + for (int i=0; i<(int)vars_.size(); i++) + vars_[i]->set(SolverDoubleParam_X, ip_solver_.optsol().x()[i]); + return exit_code; + } + + ExitCode Model::optimize() + { + ExitCode exit_code = ExitCode::Indeterminate; + + // warm start solution + if (this->getStgs().get(SolverIntParam_WarmStartIters)>0 && (numTrustRegions_>0 || numSoftConstraints_>0)) { + this->getStgs().set(SolverIntParam_MaxIters, this->getStgs().get(SolverIntParam_WarmStartIters)); + this->build_problem(0, true); + exit_code =this->solve_problem(); + } + + if (this->getStgs().get(SolverIntParam_SolverMaxIters)>0) { + // solve problem using convex conic solver + this->getStgs().set(SolverIntParam_MaxIters, this->getStgs().get(SolverIntParam_SolverMaxIters)); + this->build_problem(1); + exit_code = this->solve_problem(); + } + + // perform refinements if required + if (this->getStgs().get(SolverIntParam_NumberRefinementsTrustRegion) > 0 && (numTrustRegions_>0 || numSoftConstraints_>0)) { + for (int ref=1; ref<=this->getStgs().get(SolverIntParam_NumberRefinementsTrustRegion); ref++) { + this->getStgs().set(SolverIntParam_MaxIters, this->getStgs().get(SolverIntParam_SolverMaxIters)); + this->build_problem(ref+1); + exit_code = this->solve_problem(); + } + } + return exit_code; + } + +} diff --git a/ExternalSource/Optimizer/ConicSolver/src/solver/interface/OptVar.cpp b/ExternalSource/Optimizer/ConicSolver/src/solver/interface/OptVar.cpp new file mode 100644 index 00000000..a37572d2 --- /dev/null +++ b/ExternalSource/Optimizer/ConicSolver/src/solver/interface/OptVar.cpp @@ -0,0 +1,114 @@ +/* + * 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 { + + // Some alternative initialization functions + void OptimizationVariable::initialize(const char& type, int rows, int cols, double lBnd, double uBnd, int& startIndexInOptVec ) + { + type_ = type; rows_ = rows; cols_ = cols; + lBndMat_.resize(rows, cols); lBndMat_.setConstant(lBnd); + uBndMat_.resize(rows, cols); uBndMat_.setConstant(uBnd); + guessMat_.resize(rows, cols); guessMat_.setZero(); + indexPosition_ = startIndexInOptVec; startIndexInOptVec += rows*cols; + } + + void OptimizationVariable::initialize(const char& type, int rows, int cols, OptVector& lBnd, OptVector& uBnd, int& startIndexInOptVec ) + { + assert(rows == lBnd.rows()); + assert(rows == uBnd.rows()); + type_ = type; rows_ = rows; cols_ = cols; + lBndMat_.resize(rows, cols); lBndMat_.setZero(); lBndMat_.colwise() = lBnd.block(0,0,rows_,1); + uBndMat_.resize(rows, cols); uBndMat_.setZero(); uBndMat_.colwise() = uBnd.block(0,0,rows_,1); + guessMat_.resize(rows, cols); guessMat_.setZero(); + indexPosition_ = startIndexInOptVec; startIndexInOptVec += rows*cols; + } + + void OptimizationVariable::initialize(const char& type, int rows, int cols, OptMatrix& lBnd, OptMatrix& uBnd, int& startIndexInOptVec ) + { + assert(rows == lBnd.rows()); assert(cols == lBnd.cols()); + assert(rows == uBnd.rows()); assert(cols == uBnd.cols()); + type_ = type; rows_ = rows; cols_ = cols; + lBndMat_.resize(rows, cols); lBndMat_ = lBnd.block(0,0,rows,cols); + uBndMat_.resize(rows, cols); uBndMat_ = uBnd.block(0,0,rows,cols); + guessMat_.resize(rows, cols); guessMat_.setZero(); + indexPosition_ = startIndexInOptVec; startIndexInOptVec += rows*cols; + } + + void OptimizationVariable::initialize(const char& type, int rows, int cols, double lBnd, double uBnd, int& startIndexInOptVec, double guess) + { + guessValueInitialized_ = true; + type_ = type; rows_ = rows; cols_ = cols; + lBndMat_.resize(rows, cols); lBndMat_.setConstant(lBnd); + uBndMat_.resize(rows, cols); uBndMat_.setConstant(uBnd); + guessMat_.resize(rows, cols); guessMat_.setConstant(guess); + indexPosition_ = startIndexInOptVec; startIndexInOptVec += rows*cols; + } + + void OptimizationVariable::initialize(const char& type, int rows, int cols, double lBnd, double uBnd, int& startIndexInOptVec, OptMatrix& guess ) + { + assert(rows == guess.rows()); assert(cols == guess.cols()); + guessValueInitialized_ = true; + type_ = type; rows_ = rows; cols_ = cols; + lBndMat_.resize(rows, cols); lBndMat_.setConstant(lBnd); + uBndMat_.resize(rows, cols); uBndMat_.setConstant(uBnd); + guessMat_.resize(rows, cols); guessMat_ = guess.block(0, 0, rows, cols); + indexPosition_ = startIndexInOptVec; startIndexInOptVec += rows*cols; + } + + void OptimizationVariable::initialize(const char& type, int rows, int cols, OptVector& lBnd, OptVector& uBnd, int& startIndexInOptVec, OptMatrix& guess ) + { + assert(rows == lBnd.rows()); + assert(rows == uBnd.rows()); + assert(rows == guess.rows()); assert(cols == guess.cols()); + guessValueInitialized_ = true; + type_ = type; rows_ = rows; cols_ = cols; + lBndMat_.resize(rows, cols); lBndMat_.setZero(); lBndMat_.colwise() = lBnd.block(0,0,rows_,1); + uBndMat_.resize(rows, cols); uBndMat_.setZero(); uBndMat_.colwise() = uBnd.block(0,0,rows_,1); + guessMat_.resize(rows, cols); guessMat_ = guess.block(0, 0, rows, cols);; + indexPosition_ = startIndexInOptVec; startIndexInOptVec += rows*cols; + } + + void OptimizationVariable::initialize(const char& type, int rows, int cols, OptMatrix& lBnd, OptMatrix& uBnd, int& startIndexInOptVec, OptMatrix& guess ) + { + assert(rows == lBnd.rows()); assert(cols == lBnd.cols()); + assert(rows == uBnd.rows()); assert(cols == uBnd.cols()); + assert(rows == guess.rows()); assert(cols == guess.cols()); + guessValueInitialized_ = true; + type_ = type; rows_ = rows; cols_ = cols; + lBndMat_.resize(rows, cols); lBndMat_ = lBnd.block(0,0,rows,cols);; + uBndMat_.resize(rows, cols); uBndMat_ = uBnd.block(0,0,rows,cols);; + guessMat_.resize(rows, cols); guessMat_ = guess.block(0,0,rows,cols);; + indexPosition_ = startIndexInOptVec; startIndexInOptVec += rows*cols; + } + + int OptimizationVariable::getRowBndInd(int row) const + { + if (row >= rows_) { return rows_-1; } + else if (row <= 0) { return 0; } + else { return row; } + } + + int OptimizationVariable::getColBndInd(int col) const + { + if (col >= cols_) { return cols_-1; } + else if (col <= 0) { return 0; } + else { return col; } + } +} diff --git a/ExternalSource/Optimizer/ConicSolver/src/solver/interface/SolverSetting.cpp b/ExternalSource/Optimizer/ConicSolver/src/solver/interface/SolverSetting.cpp new file mode 100644 index 00000000..c6a1de7d --- /dev/null +++ b/ExternalSource/Optimizer/ConicSolver/src/solver/interface/SolverSetting.cpp @@ -0,0 +1,217 @@ +/* + * + * 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 +#include +#include + +namespace solver { + + void SolverSetting::initialize(const std::string cfg_file, const std::string stgs_vars_yaml) + { + try + { + YAML::Node stgs_cfg = YAML::LoadFile(cfg_file.c_str()); + YAML::Node stgs_vars = stgs_cfg[stgs_vars_yaml.c_str()]; + + // Convergence tolerances + feasibility_tolerance_ = stgs_vars["feasibility_tolerance"].as(); + absolute_suboptimality_gap_ = stgs_vars["absolute_suboptimality_gap"].as(); + relative_suboptimality_gap_ = stgs_vars["relative_suboptimality_gap"].as(); + feasibility_tolerance_inaccurate_ = stgs_vars["feasibility_tolerance_inaccurate"].as(); + absolute_suboptimality_gap_inaccurate_ = stgs_vars["absolute_suboptimality_gap_inaccurate"].as(); + relative_suboptimality_gap_inaccurate_ = stgs_vars["relative_suboptimality_gap_inaccurate"].as(); + + // Equilibration parameters + equil_iterations_ = stgs_vars["equil_iterations"].as(); + + // Linear System parameters + dyn_reg_thresh_ = stgs_vars["dyn_reg_thresh"].as(); + lin_sys_accuracy_ = stgs_vars["lin_sys_accuracy"].as(); + err_reduction_factor_ = stgs_vars["err_reduction_factor"].as(); + num_iter_ref_lin_solve_ = stgs_vars["num_iter_ref_lin_solve"].as(); + static_regularization_ = stgs_vars["static_regularization"].as(); + dynamic_regularization_ = stgs_vars["dynamic_regularization"].as(); + + // Algorithm parameters + safeguard_ = stgs_vars["safeguard"].as(); + min_step_length_ = stgs_vars["min_step_length"].as(); + max_step_length_ = stgs_vars["max_step_length"].as(); + min_centering_step_ = stgs_vars["min_centering_step"].as(); + max_centering_step_ = stgs_vars["max_centering_step"].as(); + step_length_scaling_ = stgs_vars["step_length_scaling"].as(); + + // Model parameters + verbose_ = stgs_vars["verbose"].as(); + + max_iters_ = stgs_vars["max_iters"].as(); + ipsolver_max_iters_ = stgs_vars["ipsolver_max_iters"].as(); + ipsolver_warm_iters_ = stgs_vars["ipsolver_warm_iters"].as(); + + num_itrefs_trustregion_ = stgs_vars["num_itrefs_trustregion"].as(); + trust_region_threshold_ = stgs_vars["trust_region_threshold"].as(); + soft_constraint_weight_ = stgs_vars["soft_constraint_weight"].as(); + } + catch (YAML::ParserException &e) + { + std::cout << e.what() << "\n"; + } + } + + // getter and setter methods for boolean parameters + bool SolverSetting::get(SolverBoolParam param) const + { + switch (param) + { + // Model parameters + case SolverBoolParam_Verbose: { return verbose_; } + + // Not handled parameters + default: { throw std::runtime_error("SolverSetting::get SolverBoolParam invalid"); break; } + } + } + + void SolverSetting::set(SolverBoolParam param, bool value) + { + switch (param) + { + // Model parameters + case SolverBoolParam_Verbose: { verbose_ = value; break; } + + // Not handled parameters + default: { throw std::runtime_error("SolverSetting::set SolverBoolParam invalid"); break; } + } + } + + // getter and setter methods for integer parameters + int SolverSetting::get(SolverIntParam param) const + { + switch (param) + { + // Equilibration parameters + case SolverIntParam_EquilibrationIters : { return equil_iterations_; } + + // Linear System parameters + case SolverIntParam_NumIterRefinementsLinSolve : { return num_iter_ref_lin_solve_; } + + // Model parameters + case SolverIntParam_MaxIters: { return max_iters_; } + case SolverIntParam_SolverMaxIters : { return ipsolver_max_iters_; } + case SolverIntParam_WarmStartIters : { return ipsolver_warm_iters_; } + case SolverIntParam_NumberRefinementsTrustRegion : { return num_itrefs_trustregion_; } + default: { throw std::runtime_error("SolverSetting::get SolverIntParam invalid"); break; } + } + } + + void SolverSetting::set(SolverIntParam param, int value) + { + switch (param) + { + // Equilibration parameters + case SolverIntParam_EquilibrationIters : { equil_iterations_ = value; break; } + + // Linear System parameters + case SolverIntParam_NumIterRefinementsLinSolve : { num_iter_ref_lin_solve_ = value; break; } + + // Model parameters + case SolverIntParam_MaxIters : { max_iters_ = value; break; } + case SolverIntParam_SolverMaxIters : { ipsolver_max_iters_ = value; break; } + case SolverIntParam_WarmStartIters : { ipsolver_warm_iters_ = value; break; } + case SolverIntParam_NumberRefinementsTrustRegion : { num_itrefs_trustregion_ = value; break; } + default: { throw std::runtime_error("SolverSetting::set SolverIntParam invalid"); break; } + } + } + + // getter and setter methods for double parameters + double SolverSetting::get(SolverDoubleParam param) const + { + switch (param) + { + // Convergence tolerances + case SolverDoubleParam_FeasibilityTol: { return feasibility_tolerance_; } + case SolverDoubleParam_DualityGapAbsTol: { return absolute_suboptimality_gap_; } + case SolverDoubleParam_DualityGapRelTol: { return relative_suboptimality_gap_; } + case SolverDoubleParam_FeasibilityTolInacc: { return feasibility_tolerance_inaccurate_; } + case SolverDoubleParam_DualityGapAbsTolInacc: { return absolute_suboptimality_gap_inaccurate_; } + case SolverDoubleParam_DualityGapRelTolInacc: { return relative_suboptimality_gap_inaccurate_; } + + // Linear System parameters + case SolverDoubleParam_LinearSystemAccuracy : { return lin_sys_accuracy_; } + case SolverDoubleParam_ErrorReductionFactor : { return err_reduction_factor_; } + case SolverDoubleParam_DynamicRegularizationThresh : { return dyn_reg_thresh_; } + case SolverDoubleParam_StaticRegularization : { return static_regularization_; } + case SolverDoubleParam_DynamicRegularization : { return dynamic_regularization_; } + + // Algorithm parameters + case SolverDoubleParam_SafeGuard : { return safeguard_; } + case SolverDoubleParam_MinimumStepLength : { return min_step_length_; } + case SolverDoubleParam_MaximumStepLength : { return max_step_length_; } + case SolverDoubleParam_StepLengthScaling : { return step_length_scaling_; } + case SolverDoubleParam_MinimumCenteringStep : { return min_centering_step_; } + case SolverDoubleParam_MaximumCenteringStep : { return max_centering_step_; } + + // Model parameters + case SolverDoubleParam_TrustRegionThreshold : { return trust_region_threshold_; } + case SolverDoubleParam_SoftConstraintWeight : { return soft_constraint_weight_; } + + // Not handled parameters + default: { throw std::runtime_error("SolverSetting::get SolverDoubleParam invalid"); break; } + } + } + + void SolverSetting::set(SolverDoubleParam param, double value) + { + switch (param) + { + // Convergence tolerances + case SolverDoubleParam_FeasibilityTol: { feasibility_tolerance_ = value; break; } + case SolverDoubleParam_DualityGapAbsTol: { absolute_suboptimality_gap_ = value; break; } + case SolverDoubleParam_DualityGapRelTol: { relative_suboptimality_gap_ = value; break; } + case SolverDoubleParam_FeasibilityTolInacc: { feasibility_tolerance_inaccurate_ = value; break; } + case SolverDoubleParam_DualityGapAbsTolInacc: { absolute_suboptimality_gap_inaccurate_ = value; break; } + case SolverDoubleParam_DualityGapRelTolInacc: { relative_suboptimality_gap_inaccurate_ = value; break; } + + // Linear System parameters + case SolverDoubleParam_LinearSystemAccuracy : { lin_sys_accuracy_ = value; break; } + case SolverDoubleParam_ErrorReductionFactor : { err_reduction_factor_ = value; break; } + case SolverDoubleParam_DynamicRegularizationThresh : { dyn_reg_thresh_ = value; break; } + case SolverDoubleParam_StaticRegularization : { static_regularization_ = value; break; } + case SolverDoubleParam_DynamicRegularization : { dynamic_regularization_ = value; break; } + + // Algorithm parameters + case SolverDoubleParam_SafeGuard : { safeguard_ = value; break; } + case SolverDoubleParam_MinimumStepLength : { min_step_length_ = value; break; } + case SolverDoubleParam_MaximumStepLength : { max_step_length_ = value; break; } + case SolverDoubleParam_StepLengthScaling : { step_length_scaling_ = value; break; } + case SolverDoubleParam_MinimumCenteringStep : { min_centering_step_ = value; break; } + case SolverDoubleParam_MaximumCenteringStep : { max_centering_step_ = value; break; } + + // Model parameters + case SolverDoubleParam_TrustRegionThreshold : { trust_region_threshold_ = value; break; } + case SolverDoubleParam_SoftConstraintWeight : { soft_constraint_weight_ = value; break; } + + // Not handled parameters + default: { throw std::runtime_error("SolverSetting::set SolverDoubleParam invalid"); break; } + } + } + +} diff --git a/ExternalSource/Optimizer/ConicSolver/src/solver/interface/Var.cpp b/ExternalSource/Optimizer/ConicSolver/src/solver/interface/Var.cpp new file mode 100644 index 00000000..bf8fba50 --- /dev/null +++ b/ExternalSource/Optimizer/ConicSolver/src/solver/interface/Var.cpp @@ -0,0 +1,83 @@ +/* + * 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 +#include +#include + +namespace solver { + + Var::Var(int col_no, const VarType& type, double lb, double ub, double guess) + { + var_storage_.reset( new VarStorage() ); + var_storage_->lb_ = lb; + var_storage_->ub_ = ub; + var_storage_->type_ = type; + var_storage_->value_ = guess; + var_storage_->guess_ = guess; + var_storage_->col_no_ = col_no; + } + + int Var::get(SolverIntParam param) const + { + int value; + if (var_storage_ == nullptr) + throw std::runtime_error("Variable not initialized"); + + switch (param) { + case SolverIntParam_ColNum : { value = this->var_storage_->col_no_; break; } + default: { throw std::runtime_error("Var::get IntParam"); break; } + } + return value; + } + + void Var::set(SolverIntParam param, int value) + { + switch (param) { + case SolverIntParam_ColNum : { this->var_storage_->col_no_ = value; break; } + default: { throw std::runtime_error("Var::set IntParam"); break; } + } + } + + double Var::get(SolverDoubleParam param) const + { + double value; + if (var_storage_ == nullptr) + throw std::runtime_error("Variable not initialized"); + + switch (param) { + case SolverDoubleParam_X : { value = this->var_storage_->value_; break; } + case SolverDoubleParam_LB : { value = this->var_storage_->lb_; break; } + case SolverDoubleParam_UB : { value = this->var_storage_->ub_; break; } + case SolverDoubleParam_Guess : { value = this->var_storage_->guess_; break; } + default: { throw std::runtime_error("Var::get SolverDoubleParam"); break; } + } + return value; + } + + void Var::set(SolverDoubleParam param, double value) + { + switch (param) { + case SolverDoubleParam_X : { this->var_storage_->value_ = value; break; } + case SolverDoubleParam_LB : { this->var_storage_->lb_ = value; break; } + case SolverDoubleParam_UB : { this->var_storage_->ub_ = value; break; } + case SolverDoubleParam_Guess : { this->var_storage_->guess_ = value; break; } + default: { throw std::runtime_error("Var::set SolverDoubleParam"); break; } + } + } + +} diff --git a/ExternalSource/Optimizer/ConicSolver/src/solver/optimizer/EqRoutine.cpp b/ExternalSource/Optimizer/ConicSolver/src/solver/optimizer/EqRoutine.cpp new file mode 100644 index 00000000..d8a13d58 --- /dev/null +++ b/ExternalSource/Optimizer/ConicSolver/src/solver/optimizer/EqRoutine.cpp @@ -0,0 +1,103 @@ +/* + * + * 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 { + + void EqRoutine::setEquilibration(const Cone& cone, const SolverSetting& stgs, SolverStorage& stg) + { + equil_vec_.initialize(cone); + stgs_ = std::make_shared(stgs); + cone_ = std::make_shared(cone); + this->ruizEquilibration(stg); + } + + void EqRoutine::ruizEquilibration(SolverStorage& stg) + { + Vector equil_tmp; + equil_vec_.setOnes(); + equil_tmp.initialize(*cone_); + + // iterative equilibration + for (int iter=0; iterget(SolverIntParam_EquilibrationIters); iter++) { + equil_tmp.setZero(); + + // infinity norms of rows and columns of optimization matrices + if (stg.A().nonZeros()>0) { maxRowsCols(equil_tmp.y().data(), equil_tmp.x().data(), stg.A()); } + if (stg.G().nonZeros()>0) { maxRowsCols(equil_tmp.z().data(), equil_tmp.x().data(), stg.G()); } + + // equilibration of second order cones + for (int i=0; inumSoc(); i++) { equil_tmp.zSoc(i).setConstant( equil_tmp.zSoc(i).sum() ); } + for (int i=0; isizeProb(); i++) { equil_tmp[i] = fabs(equil_tmp[i]) < 1e-6 ? 1.0 : sqrt(equil_tmp[i]); } + + // matrices equilibration + if (stg.A().nonZeros()>0) { equilibrateRowsCols(equil_tmp.y().data(), equil_tmp.x().data(), stg.A()); } + if (stg.G().nonZeros()>0) { equilibrateRowsCols(equil_tmp.z().data(), equil_tmp.x().data(), stg.G()); } + + // update equilibration vector + equil_vec_.array() *= equil_tmp.array(); + } + stg.cbh().array() /= equil_vec_.array(); + } + + void EqRoutine::maxRowsCols(double *row_vec, double *col_vec, const Eigen::SparseMatrix& mat) + { + const int* outPtr = mat.outerIndexPtr(); + for (int col=0; col0) { + Eigen::Map eig_col(&mat.valuePtr()[outPtr[col]], outPtr[col+1]-outPtr[col]); + col_vec[col] = eig_col.cwiseAbs().maxCoeff(); + for (Eigen::SparseMatrix::InnerIterator it(mat,col); it; ++it) + row_vec[it.row()] = std::max(fabs(it.value()), row_vec[it.row()]); + } + } + } + + void EqRoutine::equilibrateRowsCols(const double *row_vec, const double *col_vec, Eigen::SparseMatrix& mat) + { + for (int k=0; k::InnerIterator it(mat,k); it; ++it) + it.valueRef() /= (col_vec[it.col()] * row_vec[it.row()]); + } + + void EqRoutine::unsetEquilibration(SolverStorage& stg) + { + if (stg.A().nonZeros()>0) { unequilibrateRowsCols(equil_vec_.y().data(), equil_vec_.x().data(), stg.A()); } + if (stg.G().nonZeros()>0) { unequilibrateRowsCols(equil_vec_.z().data(), equil_vec_.x().data(), stg.G()); } + stg.cbh().array() *= equil_vec_.array(); + } + + void EqRoutine::unequilibrateRowsCols(const double *row_vec, const double *col_vec, Eigen::SparseMatrix& mat) + { + for (int k=0; k::InnerIterator it(mat,k); it; ++it) + it.valueRef() *= (row_vec[it.row()] * col_vec[it.col()]); + } + + void EqRoutine::scaleVariables(OptimizationVector& opt) + { + opt.xyz().array() /= (equil_vec_*opt.tau()).array(); + opt.s().array() *= (equil_vec_.z()/opt.tau()).array(); + } + +} diff --git a/ExternalSource/Optimizer/ConicSolver/src/solver/optimizer/IPSolver.cpp b/ExternalSource/Optimizer/ConicSolver/src/solver/optimizer/IPSolver.cpp new file mode 100644 index 00000000..c411cda8 --- /dev/null +++ b/ExternalSource/Optimizer/ConicSolver/src/solver/optimizer/IPSolver.cpp @@ -0,0 +1,419 @@ +/* + * + * 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 { + + inline double dotProduct(int n, double* x, double* y) + { + double z = 0; int i; + for( i=0; iinternalInitialization(); + } + + void InteriorPointSolver::internalInitialization() + { + // setup message printer + this->getPrinter().initialize(this->getStgs()); + + // equilibration of problem data + this->getEqRoutine().setEquilibration(this->getCone(), this->getStgs(), this->getStg()); + this->getStg().At() = this->getStg().A().transpose(); + this->getStg().Gt() = this->getStg().G().transpose(); + this->getLinSolver().initialize(this->getCone(), this->getStgs(), this->getStg()); + + // initialize problem variables + rho_.initialize(this->getCone()); + opt_.initialize(this->getCone()); + res_.initialize(this->getCone()); + RHS1_.initialize(this->getCone()); + RHS2_.initialize(this->getCone()); + lbar_.initialize(this->getCone()); + dopt1_.initialize(this->getCone()); + dopt2_.initialize(this->getCone()); + sigma_.initialize(this->getCone()); + lambda_.initialize(this->getCone()); + best_opt_.initialize(this->getCone()); + ds_combined_.initialize(this->getCone()); + dz_combined_.initialize(this->getCone()); + ds_affine_by_W_.initialize(this->getCone()); + W_times_dz_affine_.initialize(this->getCone()); + } + + ExitCode InteriorPointSolver::initializeVariables() + { + // get scalings of problem data + inires_x_ = std::max(1.0, this->getStg().c().norm()); + inires_y_ = std::max(1.0, this->getStg().b().norm()); + inires_z_ = std::max(1.0, this->getStg().h().norm()); + + // initialize KKT matrix and perform numeric factorization + this->getLinSolver().initializeMatrix(); + if ( this->getLinSolver().numericFactorization() != FactStatus::Optimal ){ + this->getPrinter().display(Msg::MatrixFactorization, this->getInfo()); + return ExitCode::Indeterminate; + } + + // initialize primal variables + int* invPerm = this->getLinSolver().invPerm().indices().data(); + for (int i=0; igetCone().numVars(); i++) { RHS1_[invPerm[i]] = 0; } + for (int i=0; igetCone().numLeq(); i++ ) { RHS1_[invPerm[this->getCone().numVars()+i]] = this->getStg().b()[i]; } + for (int i=0; igetCone().sizeLpc(); i++) { RHS1_[invPerm[this->getCone().lpConeStart()+i]] = this->getStg().h()[i]; } + for (int l=0; lgetCone().numSoc(); l++ ){ + for (int i=0; igetCone().sizeSoc(l); i++) + RHS1_[invPerm[this->getCone().extStartSoc(l)+i]] = this->getStg().cbh()[this->getCone().optStartSoc(l)+i]; + } + + this->getInfo().get(SolverIntParam_NumRefsLinSolve) = this->getLinSolver().solve(RHS1_, dopt2_, true); + opt_.x() = dopt2_.x(); + opt_.s() = -dopt2_.z(); + this->getCone().conicProjection(opt_.s()); + + // initialize dual variables + for (int i=0; igetCone().numVars(); i++){ RHS2_[invPerm[i]] = -this->getStg().c()[i]; } + + this->getInfo().get(SolverIntParam_NumRefsLinSolveAffine) = this->getLinSolver().solve(RHS2_, dopt1_, true); + opt_.y() = dopt1_.y(); + opt_.z() = dopt1_.z(); + this->getCone().conicProjection(opt_.z()); + + // initialize variables for optimization + for (int i=0; igetCone().numVars(); i++) { RHS1_[invPerm[i]] = -this->getStg().c()[i]; } + opt_.kappa() = opt_.tau() = 1.0; + this->getInfo().get(SolverDoubleParam_StepLength) = this->getInfo().get(SolverDoubleParam_AffineStepLength) = 0.0; + + return ExitCode::Optimal; + } + + void InteriorPointSolver::computeResiduals() + { + this->getLinSolver().matrixTransposeTimesVector(this->getStg().A(), opt_.y(), res_.x(), false, true); + this->getLinSolver().matrixTransposeTimesVector(this->getStg().G(), opt_.z(), res_.x(), false, false); + residual_x_ = res_.x().norm(); + + this->getLinSolver().matrixTransposeTimesVector(this->getStg().At(), opt_.x(), res_.y(), true, true); + residual_y_ = res_.y().norm(); + + this->getLinSolver().matrixTransposeTimesVector(this->getStg().Gt(), opt_.x(), res_.z(), true, true); + res_.z() += opt_.s(); + residual_z_ = res_.z().norm(); + + cx_ = this->getStg().c().dot(opt_.x()); + by_ = this->getStg().b().dot(opt_.y()); + hz_ = this->getStg().h().dot(opt_.z()); + res_ -= opt_.tau()*this->getStg().cbh(); + residual_t_ = opt_.kappa() + cx_ + by_ + hz_; + } + + double InteriorPointSolver::lineSearch(const Eigen::Ref& dsvec, const Eigen::Ref& dzvec, + double tau, double dtau, double kappa, double dkappa) + { + int conestart, conesize; + const double *lk, *dsk, *dzk; + double *lkbar, *rhok, *sigmak; + const double* ds = dsvec.data(); + const double* dz = dzvec.data(); + const double* lambda = lambda_.data(); + double conicres_lk, conicres_ds, conicres_dz, inv_conicres, factor1, factor2; + + // Linear cone + double rhomin = ds[0]/lambda[0]; + double sigmamin = dz[0]/lambda[0]; + for (int i=1; igetCone().sizeLpc(); i++){ + if (ds[i]/lambda[i]getStgs().get(SolverDoubleParam_DynamicRegularizationThresh); + + // Tau and kappa + if (-tau/dtau>0 && -tau/dtau0 && -kappa/dkappagetCone().numSoc(); i++) { + // indices + conesize = this->getCone().sizeSoc(i); + conestart = this->getCone().startSoc(i); + dsk = ds + conestart; dzk = dz + conestart; + lk = lambda + conestart; lkbar = lbar_.data() + conestart; + rhok = rho_.data() + conestart; sigmak = sigma_.data() + conestart; + + Eigen::Map eig_lk(lk,conesize); + Eigen::Map eig_rhok(rhok, conesize); + Eigen::Map eig_sigmak(sigmak, conesize); + + // find residuals + conicres_lk = this->getCone().conicResidual(eig_lk); + if (conicres_lk <= 0.0) { continue; } + + for (int j=0; jgetStgs().get(SolverDoubleParam_MaximumStepLength)),this->getStgs().get(SolverDoubleParam_MinimumStepLength)); + } + + void InteriorPointSolver::updateStatistics() + { + this->getInfo().get(SolverDoubleParam_PrimalCost) = cx_ / opt_.tau(); + this->getInfo().get(SolverDoubleParam_DualityGap) = opt_.s().dot(opt_.z()); + this->getInfo().get(SolverDoubleParam_DualCost) = -(hz_ + by_) / opt_.tau(); + this->getInfo().get(SolverDoubleParam_KappaOverTau) = opt_.kappa() / opt_.tau(); + this->getInfo().get(SolverDoubleParam_MeritFunction) = (this->getInfo().get(SolverDoubleParam_DualityGap) + opt_.kappa()*opt_.tau()) / (this->getCone().sizeCone()+1); + + // relative duality gap + if (this->getInfo().get(SolverDoubleParam_PrimalCost) < 0.0) { this->getInfo().get(SolverDoubleParam_RelativeDualityGap) = this->getInfo().get(SolverDoubleParam_DualityGap) / (-this->getInfo().get(SolverDoubleParam_PrimalCost)); } + else if (this->getInfo().get(SolverDoubleParam_DualCost) > 0.0) { this->getInfo().get(SolverDoubleParam_RelativeDualityGap) = this->getInfo().get(SolverDoubleParam_DualityGap) / this->getInfo().get(SolverDoubleParam_DualCost); } + else { this->getInfo().get(SolverDoubleParam_RelativeDualityGap) = SolverSetting::nan; } + + // residuals + this->getInfo().get(SolverDoubleParam_PrimalResidual) = std::max(res_.y().norm()/std::max(inires_y_+opt_.x().norm(),1.0), + res_.z().norm()/std::max(inires_z_+opt_.x().norm()+opt_.s().norm(),1.0)) / opt_.tau(); + this->getInfo().get(SolverDoubleParam_DualResidual) = res_.x().norm()/std::max(inires_x_+opt_.y().norm()+opt_.z().norm(),1.0) / opt_.tau(); + + // infeasibility measures + this->getInfo().get(SolverDoubleParam_PrimalInfeasibility) = (hz_ + by_)/std::max(opt_.y().norm()+opt_.z().norm(),1.0) < -this->getStgs().get(SolverDoubleParam_DualityGapRelTol) ? residual_x_ / std::max(opt_.y().norm()+opt_.z().norm(),1.0) : SolverSetting::nan; + this->getInfo().get(SolverDoubleParam_DualInfeasibility) = cx_/std::max(opt_.x().norm(),1.0) < -this->getStgs().get(SolverDoubleParam_DualityGapRelTol) ? std::max(residual_y_/std::max(opt_.x().norm(),1.0), residual_z_/std::max(opt_.x().norm()+opt_.s().norm(),1.0)) : SolverSetting::nan; + } + + ExitCode InteriorPointSolver::convergenceCheck(const PrecisionConvergence& mode) + { + this->getInfo().mode() = mode; + double feastol, abstol, reltol; + ExitCode exitcode = ExitCode::NotConverged; + + // set accuracy + if ( mode == PrecisionConvergence::Full) { + feastol = this->getStgs().get(SolverDoubleParam_FeasibilityTol); + abstol = this->getStgs().get(SolverDoubleParam_DualityGapAbsTol); + reltol = this->getStgs().get(SolverDoubleParam_DualityGapRelTol); + } else { + feastol = this->getStgs().get(SolverDoubleParam_FeasibilityTolInacc); + abstol = this->getStgs().get(SolverDoubleParam_DualityGapAbsTolInacc); + reltol = this->getStgs().get(SolverDoubleParam_DualityGapRelTolInacc); + } + + // Optimality + if ( (cx_<0.0 || by_+hz_<=abstol) && (this->getInfo().get(SolverDoubleParam_PrimalResidual)getInfo().get(SolverDoubleParam_DualResidual)getInfo().get(SolverDoubleParam_DualityGap)getInfo().get(SolverDoubleParam_RelativeDualityGap)getPrinter().display(Msg::OptimalityReached, this->getInfo()); + (mode==PrecisionConvergence::Full ? exitcode=ExitCode::Optimal : exitcode=ExitCode::OptimalInacc); + } + + // Dual infeasible + else if( (this->getInfo().get(SolverDoubleParam_DualInfeasibility)!=SolverSetting::nan) && (this->getInfo().get(SolverDoubleParam_DualInfeasibility)getPrinter().display(Msg::DualInfeasibility, this->getInfo()); + (mode==PrecisionConvergence::Full ? exitcode=ExitCode::DualInf : exitcode=ExitCode::DualInfInacc); + } + + // Primal infeasible + else if( ((this->getInfo().get(SolverDoubleParam_PrimalInfeasibility)!=SolverSetting::nan && this->getInfo().get(SolverDoubleParam_PrimalInfeasibility)getInfo().get(SolverDoubleParam_PrimalInfeasibility)getPrinter().display(Msg::PrimalInfeasibility, this->getInfo()); + (mode==PrecisionConvergence::Full ? exitcode=ExitCode::PrimalInf : exitcode=ExitCode::PrimalInfInacc); + } + return exitcode; + } + + void InteriorPointSolver::saveIterateAsBest() + { + best_opt_ = opt_; + this->getBestInfo() = this->getInfo(); + this->getBestInfo().get(SolverIntParam_NumIter) = this->getInfo().get(SolverIntParam_NumIter); + } + + void InteriorPointSolver::restoreBestIterate() + { + opt_ = best_opt_; + this->getInfo() = this->getBestInfo(); + } + + void InteriorPointSolver::rhsAffineStep() + { + const int* invPerm = this->getLinSolver().invPerm().indices().data(); + for (int i=0; igetCone().numVars(); i++ ) { RHS2_[invPerm[i]] = res_.x()[i]; } + for (int i=0; igetCone().numLeq(); i++ ) { RHS2_[invPerm[this->getCone().numVars()+i]] = -res_.y()[i]; } + for (int i=0; igetCone().sizeLpc(); i++ ) { RHS2_[invPerm[this->getCone().lpConeStart()+i]] = opt_.s()[i] - res_.z()[i]; } + + for (int l=0; lgetCone().numSoc(); l++) { + for (int i=0; i < this->getCone().sizeSoc(l); i++ ) + RHS2_[invPerm[this->getCone().extStartSoc(l)+i]] = opt_.s()[this->getCone().startSoc(l)+i] - res_.z()[this->getCone().startSoc(l)+i]; + } + } + + void InteriorPointSolver::rhsCenteringPredictorStep() + { + double* dz_combined_ptr = dz_combined_.data(); + const int* invPerm = this->getLinSolver().invPerm().indices().data(); + double factor = 1.0 - this->getInfo().get(SolverDoubleParam_CorrectionStepLength); + + for (int i=0; igetCone().numVars(); i++) { RHS2_[invPerm[i]] *= factor; } + for (int i=0; igetCone().numLeq(); i++ ) { RHS2_[invPerm[this->getCone().numVars()+i]] *= factor; } + for (int i=0; igetCone().sizeLpc(); i++) { RHS2_[invPerm[this->getCone().lpConeStart()+i]] = dz_combined_ptr[i]; } + for (int l=0; l < this->getCone().numSoc(); l++ ){ + for (int i=0; igetCone().sizeSoc(l); i++) + RHS2_[invPerm[this->getCone().extStartSoc(l)+i]] = dz_combined_ptr[this->getCone().startSoc(l)+i]; + } + } + + ExitCode InteriorPointSolver::optimize() + { + prev_pres_ = SolverSetting::nan; + exitcode_ = ExitCode::Indeterminate; + + if (initializeVariables() == ExitCode::Indeterminate) + return ExitCode::Indeterminate; + + for (this->getInfo().get(SolverIntParam_NumIter)=0; this->getInfo().get(SolverIntParam_NumIter)<=this->getStgs().get(SolverIntParam_SolverMaxIters); this->getInfo().get(SolverIntParam_NumIter)++) + { + computeResiduals(); + updateStatistics(); + this->getPrinter().display(Msg::OptimizationProgress, this->getInfo()); + + // SAFEGUARD: Bad update or DualityGap became negative + if (this->getInfo().get(SolverIntParam_NumIter)>0 && (this->getInfo().get(SolverDoubleParam_PrimalResidual)>this->getStgs().get(SolverDoubleParam_SafeGuard)*prev_pres_ || this->getInfo().get(SolverDoubleParam_DualityGap)<0)) + { + this->getPrinter().display(Msg::SearchDirection, best_info_); + restoreBestIterate(); + exitcode_ = convergenceCheck(PrecisionConvergence::Reduced); + if (exitcode_ == ExitCode::NotConverged) { + this->getPrinter().display(Msg::NumericalProblem, info_); + exitcode_ = ExitCode::PrSearchDirection; + } + break; + } + prev_pres_ = this->getInfo().get(SolverDoubleParam_PrimalResidual); + + // Check termination to full precision, mininum stepLength and maxNumIters reached + exitcode_ = convergenceCheck(PrecisionConvergence::Full); + if (exitcode_ == ExitCode::NotConverged) + { + // Mininum stepLength + if (this->getInfo().get(SolverIntParam_NumIter)>0 && this->getInfo().get(SolverDoubleParam_StepLength)==this->getStgs().get(SolverDoubleParam_MinimumStepLength)*this->getStgs().get(SolverDoubleParam_StepLengthScaling)) + { + this->getPrinter().display(Msg::LineSearchStagnation, this->getBestInfo()); + restoreBestIterate(); + exitcode_ = convergenceCheck(PrecisionConvergence::Reduced); + if (exitcode_ == ExitCode::NotConverged) { + exitcode_ = ExitCode::PrSearchDirection; + this->getPrinter().display(Msg::NumericalProblem, this->getInfo()); + } + break; + } + // Reached maxNumIters + else if (this->getInfo().get(SolverIntParam_NumIter)==this->getStgs().get(SolverIntParam_SolverMaxIters)) + { + if (!this->getInfo().isBetterThan(this->getBestInfo())) { restoreBestIterate(); } + exitcode_ = convergenceCheck(PrecisionConvergence::Reduced); + if (exitcode_ == ExitCode::NotConverged) { + exitcode_ = ExitCode::ReachMaxIters; + this->getPrinter().display(Msg::MaxItersReached, this->getInfo()); + } + break; + } + } else { + break; + } + + // SAFEGUARD: Compare statistics + if (this->getInfo().get(SolverIntParam_NumIter)==0) { saveIterateAsBest(); } + else if (this->getInfo().isBetterThan(this->getBestInfo())) { saveIterateAsBest(); } + + // update NTscalings, numeric factorization and solve linear system + if (this->getCone().updateNTScalings(this->opt_.s(), this->opt_.z(), this->lambda_)==ConeStatus::Outside) { + this->getPrinter().display(Msg::VariablesLeavingCone, this->getBestInfo()); + restoreBestIterate(); + exitcode_ = convergenceCheck(PrecisionConvergence::Reduced); + if (exitcode_ == ExitCode::NotConverged) { + this->getPrinter().display(Msg::NumericalProblem, this->getInfo()); + return ExitCode::PrSlacksLeaveCone; + } else { break; } + } + + this->getLinSolver().updateMatrix(); + if (this->getLinSolver().numericFactorization()!=FactStatus::Optimal) { + this->getPrinter().display(Msg::MatrixFactorization, this->getInfo()); + return ExitCode::Indeterminate; + } + this->getInfo().get(SolverIntParam_NumRefsLinSolve) = this->getLinSolver().solve(RHS1_, dopt2_); + + // Affine Step + rhsAffineStep(); + this->getInfo().get(SolverIntParam_NumRefsLinSolveAffine) = this->getLinSolver().solve(RHS2_, dopt1_); + + dt_denom_ = opt_.kappa()/opt_.tau() - dotProduct(this->getCone().numVars(), this->getStg().c().data(), dopt2_.x().data()) - dotProduct(this->getCone().numLeq(), this->getStg().b().data(), dopt2_.y().data()) - dotProduct(this->getCone().sizeCone(), this->getStg().h().data(), dopt2_.z().data()); + dt_affine_ = (residual_t_ - opt_.kappa() + dotProduct(this->getCone().numVars(), this->getStg().c().data(), dopt1_.x().data()) + dotProduct(this->getCone().numLeq(), this->getStg().b().data(), dopt1_.y().data()) + dotProduct(this->getCone().sizeCone(), this->getStg().h().data(), dopt1_.z().data())) / dt_denom_; + dk_affine_ = -this->opt_.kappa() - this->opt_.kappa()/this->opt_.tau()*dt_affine_; + for (int i=0; igetCone().sizeCone(); i++) { dopt1_.z()[i] += dt_affine_*dopt2_.z()[i]; } + + W_times_dz_affine_ = this->getCone().W()*dopt1_.z(); + for (int i=0; igetCone().sizeCone(); i++) { ds_affine_by_W_[i] = -W_times_dz_affine_[i] - lambda_[i]; } + this->getInfo().get(SolverDoubleParam_AffineStepLength) = lineSearch(ds_affine_by_W_, W_times_dz_affine_, this->opt_.tau(), dt_affine_, this->opt_.kappa(), dk_affine_); + this->getInfo().get(SolverDoubleParam_CorrectionStepLength) = std::max(std::min(std::pow(1.0-this->getInfo().get(SolverDoubleParam_AffineStepLength),3.0),this->getStgs().get(SolverDoubleParam_MaximumCenteringStep)),this->getStgs().get(SolverDoubleParam_MinimumCenteringStep)); + + // Centering and Corrector Step + ds_combined_ = lambda_*lambda_ + ds_affine_by_W_*W_times_dz_affine_- (this->getInfo().get(SolverDoubleParam_CorrectionStepLength)*this->getInfo().get(SolverDoubleParam_MeritFunction)); + ds_affine_by_W_ = lambda_/ds_combined_; + dz_combined_ = (this->getInfo().get(SolverDoubleParam_CorrectionStepLength)-1.0)*res_.z() + this->getCone().W()*ds_affine_by_W_; + dk_combined_ = this->opt_.kappa()*this->opt_.tau() + dk_affine_*dt_affine_ - this->getInfo().get(SolverDoubleParam_CorrectionStepLength)*this->getInfo().get(SolverDoubleParam_MeritFunction); + + rhsCenteringPredictorStep(); + this->getInfo().get(SolverIntParam_NumRefsLinSolveCorrector) = this->getLinSolver().solve(RHS2_, dopt1_); + + dopt1_.tau() = ((1-this->getInfo().get(SolverDoubleParam_CorrectionStepLength))*this->residual_t_ - dk_combined_/this->opt_.tau() + dotProduct(this->getCone().numVars(), this->getStg().c().data(), dopt1_.x().data()) + dotProduct(this->getCone().numLeq(), this->getStg().b().data(), dopt1_.y().data()) + dotProduct(this->getCone().sizeCone(), this->getStg().h().data(), dopt1_.z().data())) / dt_denom_; + dopt1_.xyz() += dopt1_.tau()*dopt2_.xyz(); + W_times_dz_affine_ = this->getCone().W()*dopt1_.z(); + for (int i=0; igetCone().sizeCone(); i++) { ds_affine_by_W_[i] = -(ds_affine_by_W_[i] + W_times_dz_affine_[i]); } + dopt1_.kappa() = -(dk_combined_ + this->opt_.kappa()*dopt1_.tau())/this->opt_.tau(); + this->getInfo().get(SolverDoubleParam_StepLength) = lineSearch(ds_affine_by_W_, W_times_dz_affine_, this->opt_.tau(), dopt1_.tau(), this->opt_.kappa(), dopt1_.kappa()) * this->getStgs().get(SolverDoubleParam_StepLengthScaling); + dopt1_.s() = this->getCone().W()*ds_affine_by_W_; + + // Update variables + opt_ += this->getInfo().get(SolverDoubleParam_StepLength) * dopt1_; + } + + this->getEqRoutine().scaleVariables(opt_); + return exitcode_; + } + +} diff --git a/ExternalSource/Optimizer/ConicSolver/src/solver/optimizer/InfoPrinter.cpp b/ExternalSource/Optimizer/ConicSolver/src/solver/optimizer/InfoPrinter.cpp new file mode 100644 index 00000000..e38e6526 --- /dev/null +++ b/ExternalSource/Optimizer/ConicSolver/src/solver/optimizer/InfoPrinter.cpp @@ -0,0 +1,246 @@ +/* + * + * 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 +#include +#include + +namespace solver { + + // OptimizatinInfo class + OptimizationInfo& OptimizationInfo::operator=(const OptimizationInfo& other) + { + if (this!=&other){ + this->get(SolverDoubleParam_Tau) = other.get(SolverDoubleParam_Tau); + this->get(SolverDoubleParam_Kappa) = other.get(SolverDoubleParam_Kappa); + this->get(SolverDoubleParam_DualCost) = other.get(SolverDoubleParam_DualCost); + this->get(SolverDoubleParam_DualityGap) = other.get(SolverDoubleParam_DualityGap); + this->get(SolverDoubleParam_PrimalCost) = other.get(SolverDoubleParam_PrimalCost); + this->get(SolverDoubleParam_KappaOverTau) = other.get(SolverDoubleParam_KappaOverTau); + this->get(SolverDoubleParam_DualResidual) = other.get(SolverDoubleParam_DualResidual); + this->get(SolverDoubleParam_MeritFunction) = other.get(SolverDoubleParam_MeritFunction); + this->get(SolverDoubleParam_PrimalResidual) = other.get(SolverDoubleParam_PrimalResidual); + this->get(SolverDoubleParam_DualInfeasibility) = other.get(SolverDoubleParam_DualInfeasibility); + this->get(SolverDoubleParam_RelativeDualityGap) = other.get(SolverDoubleParam_RelativeDualityGap); + this->get(SolverDoubleParam_PrimalInfeasibility) = other.get(SolverDoubleParam_PrimalInfeasibility); + } + return *this; + } + + bool OptimizationInfo::isBetterThan(const OptimizationInfo& info) const + { + bool better = false; + if (this->get(SolverDoubleParam_PrimalInfeasibility)!=SolverSetting::nan && this->get(SolverDoubleParam_KappaOverTau)>1.0) { + if( info.get(SolverDoubleParam_PrimalInfeasibility)!=SolverSetting::nan ) { + if ( (this->get(SolverDoubleParam_DualityGap)>0.0 && info.get(SolverDoubleParam_DualityGap)>0.0 && this->get(SolverDoubleParam_DualityGap)get(SolverDoubleParam_PrimalInfeasibility)>0.0 && this->get(SolverDoubleParam_PrimalInfeasibility)get(SolverDoubleParam_MeritFunction)>0.0 && this->get(SolverDoubleParam_MeritFunction)get(SolverDoubleParam_DualityGap)>0.0 && info.get(SolverDoubleParam_DualityGap)>0.0 && this->get(SolverDoubleParam_DualityGap)get(SolverDoubleParam_MeritFunction)>0.0 && this->get(SolverDoubleParam_MeritFunction)get(SolverDoubleParam_DualityGap)>0.0 && info.get(SolverDoubleParam_DualityGap)>0.0 && this->get(SolverDoubleParam_DualityGap)get(SolverDoubleParam_PrimalResidual)>0.0 && this->get(SolverDoubleParam_PrimalResidual)get(SolverDoubleParam_DualResidual)>0.0 && this->get(SolverDoubleParam_DualResidual)get(SolverDoubleParam_KappaOverTau)>0.0 && this->get(SolverDoubleParam_KappaOverTau)get(SolverDoubleParam_MeritFunction)>0.0 && this->get(SolverDoubleParam_MeritFunction)get(SolverBoolParam_Verbose)) { + switch (msg) + { + case Msg::MatrixFactorization: + { + std::cout << "Matrix factorization problem" << std::endl; + break; + } + case Msg::SearchDirection: + { + std::cout << "Unreliable search direction, recovering best iterate " << info.get(SolverIntParam_NumIter) << std::endl << std::endl; + break; + } + case Msg::NumericalProblem: + { + std::cout << "Numerical Problems " << std::setprecision(3) + << " (Feasibility = " << std::max(info.get(SolverDoubleParam_DualResidual), info.get(SolverDoubleParam_PrimalResidual)) + << ", RelGap = " << info.get(SolverDoubleParam_RelativeDualityGap) + << ", AbsGap = " << info.get(SolverDoubleParam_DualityGap) << ")" << std::endl << std::endl; + break; + } + case Msg::LineSearchStagnation: + { + std::cout << "Line search stagnation, recovering best iterate " << info.get(SolverIntParam_NumIter) << std::endl << std::endl; + break; + } + case Msg::VariablesLeavingCone: + { + std::cout << "Variables outside cone, recovering best iterate " << info.get(SolverIntParam_NumIter) << std::endl << std::endl; + break; + } + case Msg::MaxItersReached: + { + std::cout << "Max number of iterations reached " << std::setprecision(3) + << " (Feasibility = " << std::max(info.get(SolverDoubleParam_DualResidual), info.get(SolverDoubleParam_PrimalResidual)) + << ", RelGap = " << info.get(SolverDoubleParam_RelativeDualityGap) + << ", AbsGap = " << info.get(SolverDoubleParam_DualityGap) << ")" << std::endl << std::endl; + break; + } + case Msg::OptimalityReached: + { + std::cout << std::endl << (info.mode()==PrecisionConvergence::Full ? "OPTIMAL" : "Close to OPTIMAL") << std::scientific << std::setprecision(3) + << " (Feasibility = " << std::max(info.get(SolverDoubleParam_DualResidual), info.get(SolverDoubleParam_PrimalResidual)) + << ", RelGap = " << info.get(SolverDoubleParam_RelativeDualityGap) + << ", AbsGap = " << info.get(SolverDoubleParam_DualityGap) << ")" << std::endl << std::endl; + break; + } + case Msg::PrimalInfeasibility: + { + std::cout << std::endl << (info.mode()==PrecisionConvergence::Full ? "PRIMAL INFEASIBLE" : "Close to PRIMAL INFEASIBLE") << std::scientific << std::setprecision(3) + << " (Feasibility = " << info.get(SolverDoubleParam_PrimalInfeasibility) << ")" << std::endl << std::endl; + break; + } + case Msg::DualInfeasibility: + { + std::cout << std::endl << (info.mode()==PrecisionConvergence::Full ? "DUAL INFEASIBLE" : "Close to DUAL INFEASIBLE") << std::scientific << std::setprecision(3) + << " (Feasibility = " << info.get(SolverDoubleParam_DualInfeasibility) << ")" << std::endl << std::endl; + break; + } + case Msg::OptimizationProgress: + { + if (info.get(SolverIntParam_NumIter)==0) { + std::cout << std::endl + << " =============================== OPTIMIZATION PROGRESS =============================== " << std::endl << std::endl; + std::cout << "It pcost dcost gap pres dres k/t mu step sigma IR" << std::endl; + std::cout << std::scientific << std::setw(4) << info.get(SolverIntParam_NumIter) + << std::setw(12) << std::setprecision(3) << info.get(SolverDoubleParam_PrimalCost) + << std::setw(12) << std::setprecision(3) << info.get(SolverDoubleParam_DualCost) + << std::setw(8) << std::setprecision(0) << info.get(SolverDoubleParam_DualityGap) + << std::setw(7) << std::setprecision(0) << info.get(SolverDoubleParam_PrimalResidual) + << std::setw(7) << std::setprecision(0) << info.get(SolverDoubleParam_DualResidual) + << std::setw(7) << std::setprecision(0) << info.get(SolverDoubleParam_KappaOverTau) + << std::setw(7) << std::setprecision(0) << info.get(SolverDoubleParam_StepLength) + << std::fixed << std::setw(8) << std::setprecision(4) << "--- " + << std::scientific << std::setw(7) << std::setprecision(0) << "--- " + << std::fixed << std::setw(4) << std::setprecision(0) << info.get(SolverIntParam_NumRefsLinSolve) + << std::setw(3) << std::setprecision(0) << info.get(SolverIntParam_NumRefsLinSolveAffine) + << std::setw(3) << std::setprecision(0) << "-" << std::endl; + } else { + std::cout << std::scientific << std::setw(4) << info.get(SolverIntParam_NumIter) + << std::setw(12) << std::setprecision(3) << info.get(SolverDoubleParam_PrimalCost) + << std::setw(12) << std::setprecision(3) << info.get(SolverDoubleParam_DualCost) + << std::setw(8) << std::setprecision(0) << info.get(SolverDoubleParam_DualityGap) + << std::setw(7) << std::setprecision(0) << info.get(SolverDoubleParam_PrimalResidual) + << std::setw(7) << std::setprecision(0) << info.get(SolverDoubleParam_DualResidual) + << std::setw(7) << std::setprecision(0) << info.get(SolverDoubleParam_KappaOverTau) + << std::setw(7) << std::setprecision(0) << info.get(SolverDoubleParam_MeritFunction) + << std::fixed << std::setw(8) << std::setprecision(4) << info.get(SolverDoubleParam_StepLength) + << std::scientific << std::setw(7) << std::setprecision(0) << info.get(SolverDoubleParam_CorrectionStepLength) + << std::fixed << std::setw(4) << std::setprecision(0) << info.get(SolverIntParam_NumRefsLinSolve) + << std::setw(3) << std::setprecision(0) << info.get(SolverIntParam_NumRefsLinSolveAffine) + << std::setw(3) << std::setprecision(0) << info.get(SolverIntParam_NumRefsLinSolveCorrector) << std::endl; + } + break; + } + + } + } + } + +} diff --git a/ExternalSource/Optimizer/ConicSolver/src/solver/optimizer/LinSolver.cpp b/ExternalSource/Optimizer/ConicSolver/src/solver/optimizer/LinSolver.cpp new file mode 100644 index 00000000..b826c9ab --- /dev/null +++ b/ExternalSource/Optimizer/ConicSolver/src/solver/optimizer/LinSolver.cpp @@ -0,0 +1,322 @@ +/* + * + * 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 { + + void LinSolver::resizeProblemData() + { + int psize = cone_->extSizeProb(); + + Pe_.resize(psize); + perm_.resize(psize); + permX_.resize(psize); + permdX_.resize(psize); + invPerm_.resize(psize); + kkt_.resize(psize,psize); + + Gdx_.initialize(*cone_); + sign_.initialize(*cone_); + permKkt_.resize(psize,psize); + permSign_.initialize(*cone_); + } + + void LinSolver::buildProblem() + { + int n = cone_->numVars(); + int p = cone_->numLeq(); + + // Vector for regularization + sign_.x().setOnes(); + sign_.y().setConstant(-1.0); + sign_.z().setConstant(-1.0); + for (int i=0; inumSoc(); i++) + sign_.zSoc(i)[cone_->sizeSoc(i)+1] = 1.0; + + // Building KKT matrix + std::vector> coeffs; + static_regularization_ = stgs_->get(SolverDoubleParam_StaticRegularization); + + for (int id=0; id(id,id,static_regularization_)); + + for (int id=0; idAt().outerSize(); id++) { // KKT matrix (1,2) A' + for (Eigen::SparseMatrix::InnerIterator it(stg_->At(),id); it; ++it) + coeffs.push_back(Eigen::Triplet(it.row(),n+it.col(),it.value())); + coeffs.push_back(Eigen::Triplet(n+id,n+id,-static_regularization_)); + } + + for (int id=0; idsizeLpc(); id++) { // KKT matrix (1,3)-(3,3) G' and -Weights + for (Eigen::SparseMatrix::InnerIterator it(stg_->Gt(),id); it; ++it) + coeffs.push_back(Eigen::Triplet(it.row(),n+p+it.col(),it.value())); + coeffs.push_back(Eigen::Triplet(n+p+id,n+p+id,-1.0)); + cone_->indexLpc(id) = n+p+stg_->At().nonZeros()+stg_->Gt().leftCols(id+1).nonZeros()+id; + } + + int k = n+p+stg_->At().nonZeros()+stg_->Gt().leftCols(cone_->sizeLpc()).nonZeros()+cone_->sizeLpc(); + + for (int l=0; lnumSoc(); l++) { + for (int id=0; idsizeSoc(l); id++) { + for (Eigen::SparseMatrix::InnerIterator it(stg_->Gt(),cone_->startSoc(l)+id); it; ++it) + coeffs.push_back(Eigen::Triplet(it.row(),n+p+2*l+it.col(),it.value())); + coeffs.push_back(Eigen::Triplet(n+p+cone_->startSoc(l)+2*l+id,n+p+cone_->startSoc(l)+2*l+id,-1.0)); + cone_->soc(l).indexSoc(id) = k+stg_->Gt().col(cone_->startSoc(l)+id).nonZeros(); + + k += stg_->Gt().col(cone_->startSoc(l)+id).nonZeros()+1; + } + k += 2*cone_->sizeSoc(l)+1; + + for (int id=1; idsizeSoc(l); id++) + coeffs.push_back(Eigen::Triplet(n+p+cone_->startSoc(l) + 2*l + id,n+p+cone_->startSoc(l)+2*l+cone_->sizeSoc(l), 0.0)); + coeffs.push_back(Eigen::Triplet(n+p+cone_->startSoc(l)+2*l+cone_->sizeSoc(l),n+p+cone_->startSoc(l)+2*l+cone_->sizeSoc(l),-1.0)); + + for (int id=0; idsizeSoc(l); id++) + coeffs.push_back(Eigen::Triplet(n+p+cone_->startSoc(l)+2*l+id,n+p+cone_->startSoc(l)+2*l+cone_->sizeSoc(l)+1,0.0)); + coeffs.push_back(Eigen::Triplet(n+p+cone_->startSoc(l)+2*l+cone_->sizeSoc(l)+1,n+p+cone_->startSoc(l)+2*l+cone_->sizeSoc(l)+1,1.0)); + } + + kkt_.setFromTriplets(coeffs.begin(), coeffs.end()); + if (!kkt_.isCompressed()) { kkt_.makeCompressed(); } + } + + void LinSolver::findPermutation() + { + // find permutation and inverse permutation + Eigen::AMDOrdering ordering; + ordering(kkt_, perm_); + invPerm_ = perm_.inverse(); + + // permute quantities + for (int i=0; iextSizeProb(); i++) { permSign_[i] = sign_[perm_.indices()[i]]; } + permKkt_.selfadjointView() = kkt_.selfadjointView().twistedBy(invPerm_); + } + + void LinSolver::symbolicFactorization() + { + chol_.analyzePattern(permKkt_, *stgs_); + } + + FactStatus LinSolver::numericFactorization() + { + int status = chol_.factorize(this->permKkt_, this->permSign_); + return (status == this->permKkt_.cols() ? FactStatus::Optimal : FactStatus::Failure); + } + + int LinSolver::solve(const Eigen::Ref& permB, OptimizationVector& searchDir, bool is_initialization) + { + int numRefs; + int nK = cone_->extSizeProb(); + int mext = cone_->extSizeCone(); + + double* Gdx = Gdx_.data(); + Eigen::VectorXd permdZ(mext); + ExtendedVector err; err.initialize(*cone_); + int* Pinv = this->invPerm_.indices().data(); + double errNorm_cur, errNorm_prev = SolverSetting::nan; + double errThresh = (1.0 + (nK>0 ? permB.lpNorm() : 0.0 ))*stgs_->get(SolverDoubleParam_LinearSystemAccuracy); + + double* ez = err.z().data(); + double* dz = searchDir.z().data(); + + // solve perturbed linear system + chol_.solve(permB, permX_.data()); + + // iterative refinement due to regularization to KKT matrix factorization + for (numRefs=0; numRefs <= stgs_->get(SolverIntParam_NumIterRefinementsLinSolve); numRefs++) + { + cone_->unpermuteSolution(invPerm_, permX_, searchDir, permdZ); + for (int i=0; iA(), searchDir.y(), err.x(), false, false); + matrixTransposeTimesVector(stg_->G(), searchDir.z(), err.x(), false, false); + err.x() -= static_regularization_*searchDir.x(); + + // error_y = b_y - (A dx - Is dy) + if (stg_->A().nonZeros()>0) { + matrixTransposeTimesVector(stg_->At(), searchDir.x(), err.y(), false, false); + err.y() += static_regularization_*searchDir.y(); + } + + // error_z = b_z - (G dx +(Is+W2) dz) + matrixTransposeTimesVector(stg_->Gt(), searchDir.x(), Gdx_, true, true); + err.zLpc() += -Gdx_.zLpc() + static_regularization_*searchDir.zLpc(); + for (int i=0; inumSoc(); i++) { + for (int j=0; jsizeSoc(i)-1; j++) + ez[cone_->startSoc(i)+2*i+j] += -Gdx[cone_->startSoc(i)+j] + static_regularization_*dz[cone_->startSoc(i)+j]; + ez[cone_->startSoc(i)+2*i+cone_->sizeSoc(i)-1] -= Gdx[cone_->startSoc(i)+cone_->sizeSoc(i)-1] + static_regularization_*dz[cone_->startSoc(i)+cone_->sizeSoc(i)-1]; + } + if (is_initialization) { err.z() += permdZ; } + else { cone_->conicNTScaling2(permdZ, err.z()); } + + // progress checks + errNorm_cur = err.size()>0 ? err.lpNorm() : 0.0; + if (numRefs>0 && errNorm_cur>errNorm_prev ) + { permX_ -= permdX_; numRefs--; break; } + if (numRefs==stgs_->get(SolverIntParam_NumIterRefinementsLinSolve) || (errNorm_cur0 && errNorm_prevget(SolverDoubleParam_ErrorReductionFactor)*errNorm_cur)) { break; } + errNorm_prev = errNorm_cur; + + // solve and add refinement to permX + for (int i=0; iunpermuteSolution(invPerm_, permX_, searchDir); + return numRefs; + } + + void LinSolver::initialize(Cone& cone, SolverSetting& stgs, SolverStorage& stg) + { + stg_ = &stg; + stgs_ = &stgs; + cone_ = &cone; + + this->resizeProblemData(); + this->buildProblem(); + this->findPermutation(); + this->symbolicFactorization(); + + // update indices of NT scalings to access them directly in permuted matrix + Eigen::VectorXi index = Eigen::Map(permKkt_.outerIndexPtr(), cone_->extSizeProb()+1); + permK_.resize(kkt_.nonZeros()); + int nnz = 0; + for (int j=0; j::InnerIterator it(kkt_,j); it; ++it) + if (it.row()<=it.col()) { + permK_.indices()[nnz] = index[invPerm_.indices()[it.row()] > invPerm_.indices()[it.col()] ? + invPerm_.indices()[it.row()] : invPerm_.indices()[it.col()]]++; + nnz += 1; + } + + for (int id=0; idsizeLpc(); id++) + cone_->indexLpc(id) = permK_.indices()[cone_->indexLpc(id)]; + + for (int i=0; inumSoc(); i++) { + int j=1; + for (int k=0; k<2*cone_->sizeSoc(i)+1; k++) + cone_->soc(i).indexSoc(cone_->sizeSoc(i)+k) = permK_.indices()[cone_->soc(i).indexSoc(cone_->sizeSoc(i)-1)+j++]; + for (int k=0; ksizeSoc(i); k++) + cone_->soc(i).indexSoc(k) = permK_.indices()[cone_->soc(i).indexSoc(k)]; + } + } + + void LinSolver::initializeMatrix() + { + double* value = permKkt_.valuePtr(); + + // Linear cone + for (int i=0; isizeLpc(); i++) + value[cone_->indexLpc(i)] = -1.0; + + // Second order cone + for (int i=0; inumSoc(); i++) { + value[cone_->soc(i).indexSoc(0)] = -1.0; + for (int k=1; ksizeSoc(i); k++) + value[cone_->soc(i).indexSoc(k)] = -1.0; + + for (int k=0; ksizeSoc(i)-1; k++) + value[cone_->soc(i).indexSoc(cone_->sizeSoc(i)+k)] = 0.0; + value[cone_->soc(i).indexSoc(2*cone_->sizeSoc(i)-1)] = -1.0; + + value[cone_->soc(i).indexSoc(2*cone_->sizeSoc(i))] = 0.0; + for (int k=0; ksizeSoc(i)-1; k++) + value[cone_->soc(i).indexSoc(2*cone_->sizeSoc(i)+1+k)] = 0.0; + value[cone_->soc(i).indexSoc(3*cone_->sizeSoc(i))] = +1.0; + } + } + + void LinSolver::updateMatrix() + { + static_regularization_ = stgs_->get(SolverDoubleParam_StaticRegularization); + int conesize; + double eta_square, *scaling_soc; + double* value = permKkt_.valuePtr(); + + // Linear cone + for (int i=0; isizeLpc(); i++) + value[cone_->indexLpc(i)] = -cone_->sqScalingLpc(i) - static_regularization_; + + // Second order cone + for (int i=0; inumSoc(); i++) { + conesize = cone_->sizeSoc(i); + eta_square = cone_->soc(i).etaSquare(); + scaling_soc = cone_->soc(i).scalingSoc().data(); + + value[cone_->soc(i).indexSoc(0)] = -eta_square * cone_->soc(i).d1() - static_regularization_; + for (int k=1; ksoc(i).indexSoc(k)] = -eta_square - static_regularization_; + + for (int k=0; ksoc(i).indexSoc(conesize+k)] = -eta_square * cone_->soc(i).v1() * scaling_soc[k+1]; + value[cone_->soc(i).indexSoc(2*conesize-1)] = -eta_square; + + value[cone_->soc(i).indexSoc(2*conesize)] = -eta_square * cone_->soc(i).u0(); + for (int k=0; ksoc(i).indexSoc(2*conesize+1+k)] = -eta_square * cone_->soc(i).u1() * scaling_soc[k+1]; + value[cone_->soc(i).indexSoc(3*conesize)] = +eta_square + static_regularization_; + } + } + + void LinSolver::matrixTransposeTimesVector(const Eigen::SparseMatrix& A, + const Eigen::Ref& eig_x, + Eigen::Ref eig_y, + bool add, bool is_new) + { + double ylocal; + int row, inner_start, inner_end; + if (is_new) { eig_y.setZero(); } + + double* y = eig_y.data(); + const double* x = eig_x.data(); + const double* valPtr = A.valuePtr(); + const int* outPtr = A.outerIndexPtr(); + const int* innPtr = A.innerIndexPtr(); + + if (add) { + for (int col=0; colinner_start) { + for (row=inner_start; rowinner_start) { + for (row=inner_start; row + +namespace linalg { + + void SparseCholesky::analyzePattern(const Eigen::SparseMatrix& mat, const solver::SolverSetting& stgs) + { + n_ = mat.cols(); + X_.resize(n_); + D_.resize(n_); + Y_.resize(n_); + Flag_.resize(n_); + Lnnz_.resize(n_); + L_.resize(n_,n_); + Parent_.resize(n_); + Pattern_.resize(n_); + std::vector> coeffs; + stgs_ = std::make_shared(stgs); + + int* Lnnz = Lnnz_.data(); + int* Flag = Flag_.data(); + int* Parent = Parent_.data(); + + for (int k=0; k::InnerIterator it(mat,k); it; ++it) + for (int row=it.row(); Flag[row]!=k ; row=Parent[row]){ + if (Parent[row]==-1) { Parent[row]=k; } + Lnnz[row]++ ; + Flag[row] = k; + } + } + + for (int col=0; col(int(row%n_),col,1.0)); + L_.setFromTriplets(coeffs.begin(), coeffs.end()); + if (!L_.isCompressed()) { L_.makeCompressed(); } + } + + int SparseCholesky::factorize(const Eigen::SparseMatrix& mat, const Eigen::Ref& sign) + { + double* D = D_.data(); + double* Y = Y_.data(); + int* Lnnz = Lnnz_.data(); + int* Flag = Flag_.data(); + int* Parent = Parent_.data(); + int* Pattern = Pattern_.data(); + + double* Lx = L_.valuePtr(); + int* Lp = L_.outerIndexPtr(); + int* Li = L_.innerIndexPtr(); + + const double* Ax = mat.valuePtr(); + const int* Ap = mat.outerIndexPtr(); + const int* Ai = mat.innerIndexPtr(); + + int p, len; + delta_ = stgs_->get(solver::SolverDoubleParam_DynamicRegularization); + eps_ = stgs_->get(solver::SolverDoubleParam_DynamicRegularizationThresh); + + for (int k=0; k0) Pattern[--top] = Pattern[--len] ; + } + + // numerical values kth row of L + D[k] = Y[k]; + Y[k] = 0.0; + for (; top& b, double* x) + { + double* D = D_.data(); + double* Lx = L_.valuePtr(); + int* Lp = L_.outerIndexPtr(); + int* Li = L_.innerIndexPtr(); + + int n = b.size(); + Eigen::Map eig_x(x, b.size()); + eig_x = b; + for (int j=0; j=0; j--) + for (int p=Lp[j]; p::InnerIterator it(L_,k); it; ++it) + X_[it.row()] -= it.value()*X_[it.col()]; + + X_.array() /= D_.array(); + for (int k=L_.outerSize()-1; k>=0; k--) + for (Eigen::SparseMatrix::InnerIterator it(L_,k); it; ++it) + X_[it.col()] -= it.value()*X_[it.row()]; + + return X_; + } + +} diff --git a/PnC/CentroidPlanner/CMakeLists.txt b/PnC/CentroidPlanner/CMakeLists.txt index 73a971ce..4c4cbd98 100644 --- a/PnC/CentroidPlanner/CMakeLists.txt +++ b/PnC/CentroidPlanner/CMakeLists.txt @@ -3,4 +3,5 @@ file(GLOB_RECURSE srcs "*.cpp" "*.hpp") add_library(myCentPlanner SHARED ${srcs}) target_link_libraries(myCentPlanner myUtils - myContact) + myContact + myConicSolver) diff --git a/PnC/CentroidPlanner/CentroidPlanner.cpp b/PnC/CentroidPlanner/CentroidPlanner.cpp index 4f553d39..03715f0e 100644 --- a/PnC/CentroidPlanner/CentroidPlanner.cpp +++ b/PnC/CentroidPlanner/CentroidPlanner.cpp @@ -2,16 +2,19 @@ #include "Utilities.hpp" using namespace myUtils; +using namespace solver; void CentroidPlannerParameter::paramSetFromYaml(const std::string & cfg_file) { cfgFile = cfg_file; + saveDynamicsFile = cfg_file.substr(0, cfg_file.size()-5) + "_RESULT.yaml"; + defaultSolverSettingFile = THIS_COM + std::string("Config/Draco/DEFAULT_CONIC_SOLVER_SETTING.yaml"); try { YAML::Node planner_cfg = YAML::LoadFile(cfgFile.c_str()); // =============== // Planner Setting // =============== - YAML::Node planner_vars = planner_cfg["planner_variable"]; + YAML::Node planner_vars = planner_cfg["planner_variables"]; // Dynamics parameters readParameter(planner_vars, "num_com_viapoints", numComViaPoints); comViaPoints.clear(); @@ -45,9 +48,9 @@ void CentroidPlannerParameter::paramSetFromYaml(const std::string & cfg_file) { readParameter(planner_vars, "time_range", timeRange); readParameter(planner_vars, "is_time_horizon_fixed", isTimeHorizonFixed); } - for (int eff_id=0; eff_id(planner_vars, "friction_cone").compare("LinearCone")==0); // Dynamics weights @@ -75,6 +78,9 @@ void CentroidPlannerParameter::paramSetFromYaml(const std::string & cfg_file) { readParameter(planner_vars, "store_data", isStoreData); // Solver setting readParameter(planner_vars, "use_default_solver_setting", isDefaultSolverSetting); + massTimesGravity = robotMass * gravity; + gravityVector = Eigen::Vector3d(0., 0., -gravity); + numTimeSteps = std::floor(timeHorizon/timeStep); // =================== // Robot Initial State @@ -83,14 +89,19 @@ void CentroidPlannerParameter::paramSetFromYaml(const std::string & cfg_file) { readParameter(ini_robo_cfg, "com", initialState.com); readParameter(ini_robo_cfg, "lmom", initialState.lMom); readParameter(ini_robo_cfg, "amom", initialState.aMom); - for (int eff_id=0; eff_id(ini_robo_cfg["eef_pose"], "eef_"+CentroidModel::eEfIdToString(eff_id)); - initialState.eEfsActivation[eff_id] = int(eef_cfg(0)); - initialState.eEfsPosition[eff_id] = eef_cfg.segment<3>(1); - initialState.eEfsOrientation[eff_id] = Eigen::Quaternion(eef_cfg[4],eef_cfg[5],eef_cfg[6],eef_cfg[7]); - readParameter(ini_robo_cfg["eef_ctrl"], "eef_frc_"+CentroidModel::eEfIdToString(eff_id), initialState.eEfsFrc[eff_id]); + for (int eef_id=0; eef_id(ini_robo_cfg["eef_pose"], "eef_"+CentroidModel::eEfIdToString(eef_id)); + initialState.eEfsActivation[eef_id] = int(eef_cfg(0)); + initialState.eEfsPosition[eef_id] = eef_cfg.segment<3>(1); + initialState.eEfsOrientation[eef_id] = Eigen::Quaternion(eef_cfg[4],eef_cfg[5],eef_cfg[6],eef_cfg[7]); + readParameter(ini_robo_cfg["eef_ctrl"], "eef_frc_"+CentroidModel::eEfIdToString(eef_id), initialState.eEfsFrc[eef_id]); } + // =========================== + // Reference Dynamics Sequence + // =========================== + refDynamicsStateSequence.resize(numTimeSteps); + // ================ // Contact Interface // ================ @@ -98,19 +109,19 @@ void CentroidPlannerParameter::paramSetFromYaml(const std::string & cfg_file) { // Contact parameters readParameter(contact_vars, "num_contacts", contactPlanInterface.contactsPerEndeff); - for (int eff_id=0; eff_id0) { - YAML::Node eff_params = contact_vars[("effcnt_" + CentroidModel::eEfIdToString(eff_id)).c_str()]; - for (int cnt_id=0; cnt_id0) { + YAML::Node eff_params = contact_vars[("eefcnt_" + CentroidModel::eEfIdToString(eef_id)).c_str()]; + for (int cnt_id=0; cnt_id(2); - contactPlanInterface.contactSequence.eEfContacts[eff_id][cnt_id].contactType = idToContactType(v(9)); - contactPlanInterface.contactSequence.eEfContacts[eff_id][cnt_id].orientation = Eigen::Quaternion(v[5],v[6],v[7],v[8]); + contactPlanInterface.contactSequence.eEfContacts[eef_id].push_back(ContactState()); + contactPlanInterface.contactSequence.eEfContacts[eef_id][cnt_id].timeIni = v[0]; + contactPlanInterface.contactSequence.eEfContacts[eef_id][cnt_id].timeEnd = v[1]; + contactPlanInterface.contactSequence.eEfContacts[eef_id][cnt_id].position = v.segment<3>(2); + contactPlanInterface.contactSequence.eEfContacts[eef_id][cnt_id].contactType = idToContactType(v(9)); + contactPlanInterface.contactSequence.eEfContacts[eef_id][cnt_id].orientation = Eigen::Quaternion(v[5],v[6],v[7],v[8]); } } } @@ -122,12 +133,706 @@ void CentroidPlannerParameter::paramSetFromYaml(const std::string & cfg_file) { } -CentroidPlanner::CentroidPlanner() {} +CentroidPlanner::CentroidPlanner() { +} + CentroidPlanner::~CentroidPlanner() {} -void CentroidPlanner::_doPlan() {} + +void CentroidPlanner::_doPlan() { + mCentParam = std::dynamic_pointer_cast(mParam); + _initialize(); + _optimize(); +} + void CentroidPlanner::_evalTrajectory(double time, Eigen::VectorXd & pos, Eigen::VectorXd & vel, Eigen::VectorXd &trq) { } + +void CentroidPlanner::_initialize() { + if (!mCentParam->isDefaultSolverSetting) { + mModel.configSetting(mCentParam->cfgFile); + } else { + mModel.configSetting(mCentParam->defaultSolverSettingFile); + } + + mComPosGoal = mCentParam->initialState.com + mCentParam->comDisplacement; + mFrictionCone.getCone(mCentParam->frictionCoeff, mConeMatrix); + mDynStateSeq.resize(mCentParam->numTimeSteps); + for (int time_id=0; time_idnumTimeSteps; time_id++) + mDynStateSeq.dynamicsStateSequence[time_id].time = mCentParam->timeStep; + + mCentParam->contactPlanInterface.fillDynamicsSequence(mDynStateSeq, + mCentParam->numTimeSteps, + mCentParam->timeStep); + _initializeOptimizationVariables(); +} + +void CentroidPlanner::_initializeOptimizationVariables() { + mNumVars = 0.; + double inf_value = SolverSetting::inf; + + // center of mass, linear and angular momentum + mCom.initialize('C', 3, mCentParam->numTimeSteps, -inf_value, inf_value, mNumVars); + mLMom.initialize('C', 3, mCentParam->numTimeSteps, -inf_value, inf_value, mNumVars); + mAMom.initialize('C', 3, mCentParam->numTimeSteps, -inf_value, inf_value, mNumVars); + + // time variable, linear and angular momentum rates + if (mCentParam->heuristic == Heuristic::timeOptimization) { + mDt.initialize('C', 1, mCentParam->numTimeSteps, -inf_value, inf_value, mNumVars); + mLMomD.initialize('C', 3, mCentParam->numTimeSteps, -inf_value, inf_value, mNumVars); + mAMomD.initialize('C', 3, mCentParam->numTimeSteps, -inf_value, inf_value, mNumVars); + } + + // upper and lower bound variables, forces, cops, torques + for (int eef_id=0; eef_idnumActEEfs; eef_id++) { + mLbVar[eef_id].initialize('C', 3, mDynStateSeq.activeEEfSteps[eef_id], -inf_value, inf_value, mNumVars); + mUbVar[eef_id].initialize('C', 3, mDynStateSeq.activeEEfSteps[eef_id], -inf_value, inf_value, mNumVars); + mFrcWorld[eef_id].initialize('C', 3, mDynStateSeq.activeEEfSteps[eef_id], -inf_value, inf_value, mNumVars); + mCopLocal[eef_id].initialize('C', 2, mDynStateSeq.activeEEfSteps[eef_id], -inf_value, inf_value, mNumVars); + mTrqLocal[eef_id].initialize('C', 1, mDynStateSeq.activeEEfSteps[eef_id], -inf_value, inf_value, mNumVars); + } +} + +solver::ExitCode CentroidPlanner::_optimize(bool update_tracking_objective) { + if (update_tracking_objective) _updateTrackingObjective(); + + mSolveTime = 0.0; + mHasConverged = false; + _internalOptimize(true); + + if (mCentParam->heuristic == Heuristic::timeOptimization) { + for (int iter_id=1; iter_id <= mCentParam->maxTimeIterations; iter_id++) { + _internalOptimize(); + if (mHasConverged) break; + } + } + + _saveToFile(mCentParam->refDynamicsStateSequence); + return mExitCode; +} + +void CentroidPlanner::_updateTrackingObjective() { + mCentParam->wLMom = mCentParam->wLMomTrack; + mCentParam->wAMom = mCentParam->wAMomTrack; +} + +void CentroidPlanner::_internalOptimize(bool is_first_time) { + try + { + // add variables to model + mVars.clear(); + for (int var_id=0; var_idheuristic == Heuristic::timeOptimization) { + _addVariableToModel(mDt, mModel, mVars); + _addVariableToModel(mLMomD, mModel, mVars); + _addVariableToModel(mAMomD, mModel, mVars); + } + + for (int eef_id=0; eef_id < mCentParam->numActEEfs; eef_id++) { + _addVariableToModel(mLbVar[eef_id], mModel, mVars); + _addVariableToModel(mUbVar[eef_id], mModel, mVars); + _addVariableToModel(mFrcWorld[eef_id], mModel, mVars); + _addVariableToModel(mCopLocal[eef_id], mModel, mVars); + _addVariableToModel(mTrqLocal[eef_id], mModel, mVars); + } + + // adding quadratic objective + mQuadObjective.clear(); + + if (mCentParam->numComViaPoints > 0) { + for (int via_id=0; via_id < mCentParam->numComViaPoints; via_id++) + for (int axis_id=0; axis_id<3; axis_id++) + mQuadObjective.addQuaTerm(mCentParam->wComVia[axis_id], LinExpr(mVars[mCom.id(axis_id,int(mCentParam->comViaPoints[via_id](0)/mCentParam->timeStep))]) - LinExpr(mCentParam->comViaPoints[via_id](axis_id+1)) ); + } + + for (int time_id=0; time_id < mCentParam->numTimeSteps; time_id++) { + for (int axis_id=0; axis_id<3; axis_id++) { + + // penalty on center of mass, linear and angular momentum + if (time_id == mCentParam->numTimeSteps-1) { + mQuadObjective.addQuaTerm(mCentParam->wCom[axis_id], LinExpr(mVars[mCom.id(axis_id,time_id)]) - LinExpr(mComPosGoal[axis_id])); + mQuadObjective.addQuaTerm(mCentParam->wLMomFinal[axis_id], LinExpr(mVars[mLMom.id(axis_id,time_id)]) - LinExpr(mCentParam->refDynamicsStateSequence.dynamicsStateSequence[time_id].lMom[axis_id])); + mQuadObjective.addQuaTerm(mCentParam->wAMomFinal[axis_id], LinExpr(mVars[mAMom.id(axis_id,time_id)]) - LinExpr(mCentParam->refDynamicsStateSequence.dynamicsStateSequence[time_id].aMom[axis_id])); + } else { + mQuadObjective.addQuaTerm(mCentParam->wLMom[axis_id], LinExpr(mVars[mLMom.id(axis_id,time_id)]) - LinExpr(mCentParam->refDynamicsStateSequence.dynamicsStateSequence[time_id].lMom[axis_id])); + mQuadObjective.addQuaTerm(mCentParam->wAMom[axis_id], LinExpr(mVars[mAMom.id(axis_id,time_id)]) - LinExpr(mCentParam->refDynamicsStateSequence.dynamicsStateSequence[time_id].aMom[axis_id])); + } + + // penalty on linear and angular momentum rates + if (mCentParam->heuristic == Heuristic::timeOptimization) { + mQuadObjective.addQuaTerm(mCentParam->wLMomD[axis_id], mVars[mLMomD.id(axis_id,time_id)]); + mQuadObjective.addQuaTerm(mCentParam->wAMomD[axis_id], mVars[mAMomD.id(axis_id,time_id)]); + } else { + if (time_id==0) { + mQuadObjective.addQuaTerm(mCentParam->wLMomD[axis_id], (LinExpr(mVars[mLMom.id(axis_id,time_id)]) - LinExpr(mCentParam->initialState.lMom[axis_id]))*(1.0/mDynStateSeq.dynamicsStateSequence[time_id].time)); + mQuadObjective.addQuaTerm(mCentParam->wAMomD[axis_id], (LinExpr(mVars[mAMom.id(axis_id,time_id)]) - LinExpr(mCentParam->initialState.aMom[axis_id]))*(1.0/mDynStateSeq.dynamicsStateSequence[time_id].time)); + } else { + mQuadObjective.addQuaTerm(mCentParam->wLMomD[axis_id], (LinExpr(mVars[mLMom.id(axis_id,time_id)]) - LinExpr(mVars[mLMom.id(axis_id,time_id-1)]))*(1.0/mDynStateSeq.dynamicsStateSequence[time_id].time)); + mQuadObjective.addQuaTerm(mCentParam->wAMomD[axis_id], (LinExpr(mVars[mAMom.id(axis_id,time_id)]) - LinExpr(mVars[mAMom.id(axis_id,time_id-1)]))*(1.0/mDynStateSeq.dynamicsStateSequence[time_id].time)); + } + } + + // penalty on forces + for (int eef_id=0; eef_idnumActEEfs; eef_id++) { + if (mDynStateSeq.dynamicsStateSequence[time_id].eEfsActivation[eef_id]) { + if (eef_id==static_cast(CentroidModel::EEfID::rightFoot) || eef_id==static_cast(CentroidModel::EEfID::leftFoot)) { mQuadObjective.addQuaTerm(mCentParam->wFrcLeg[axis_id], mVars[mFrcWorld[eef_id].id(axis_id,mDynStateSeq.dynamicsStateSequence[time_id].eEfsActivationIds[eef_id])]); } + else { mQuadObjective.addQuaTerm(mCentParam->wFrcArm[axis_id], mVars[mFrcWorld[eef_id].id(axis_id,mDynStateSeq.dynamicsStateSequence[time_id].eEfsActivationIds[eef_id])]); } + } + } + + // penalty on rate of forces + for (int eef_id=0; eef_id < mCentParam->numActEEfs; eef_id++) { + Eigen::Vector3d ctrl_rate_penalty = Eigen::Vector3d::Zero(); + if (CentroidModel::isHand(eef_id)) { ctrl_rate_penalty = mCentParam->wFrcArm; } + else { ctrl_rate_penalty = mCentParam->wDFrcLeg; } + + if (mDynStateSeq.dynamicsStateSequence[time_id].eEfsActivation[eef_id]) { + // current force + LinExpr current_force = mVars[mFrcWorld[eef_id].id(axis_id,mDynStateSeq.dynamicsStateSequence[time_id].eEfsActivationIds[eef_id])]; + + // next force + LinExpr next_force = 0.0; + if (time_id == mCentParam->numTimeSteps-1) { next_force = current_force; } + else { + if (mDynStateSeq.dynamicsStateSequence[time_id+1].eEfsActivation[eef_id]) + next_force = mVars[mFrcWorld[eef_id].id(axis_id,mDynStateSeq.dynamicsStateSequence[time_id+1].eEfsActivationIds[eef_id])]; + } + + // previous force + LinExpr previous_force = 0.0; + if (time_id==0) { previous_force = mCentParam->initialState.eEfsFrc[eef_id][axis_id]; } + else { + if (mDynStateSeq.dynamicsStateSequence[time_id-1].eEfsActivation[eef_id]) + previous_force = mVars[mFrcWorld[eef_id].id(axis_id,mDynStateSeq.dynamicsStateSequence[time_id-1].eEfsActivationIds[eef_id])]; + } + + // penalty on force smoothness + mQuadObjective.addQuaTerm(ctrl_rate_penalty[axis_id], next_force - current_force); + mQuadObjective.addQuaTerm(ctrl_rate_penalty[axis_id], current_force - previous_force); + } + } + } + + // penalty on torques + for (int eef_id=0; eef_id < mCentParam->numActEEfs; eef_id++) { + if (mDynStateSeq.dynamicsStateSequence[time_id].eEfsActivation[eef_id]) { + if (eef_id==static_cast(CentroidModel::EEfID::rightFoot) || eef_id==static_cast(CentroidModel::EEfID::leftFoot)) { mQuadObjective.addQuaTerm(mCentParam->wTrqLeg, mVars[mTrqLocal[eef_id].id(0,mDynStateSeq.dynamicsStateSequence[time_id].eEfsActivationIds[eef_id])]); } + else { mQuadObjective.addQuaTerm(mCentParam->wTrqArm, mVars[mTrqLocal[eef_id].id(0,mDynStateSeq.dynamicsStateSequence[time_id].eEfsActivationIds[eef_id])]); } + } + } + + // penalty on time + if (mCentParam->heuristic == Heuristic::timeOptimization) { + mQuadObjective.addQuaTerm(mCentParam->wTimePenalty, LinExpr(mVars[mDt.id(0,time_id)])); + mQuadObjective.addQuaTerm(mCentParam->wTime, LinExpr(mVars[mDt.id(0,time_id)]) - LinExpr(mCentParam->timeStep)); + } + } + + if (!is_first_time && mCentParam->heuristic == Heuristic::timeOptimization) { + for (int time_id=0; time_idnumTimeSteps; time_id++) { + for (int eef_id=0; eef_idnumActEEfs; eef_id++) { + if (mDynStateSeq.dynamicsStateSequence[time_id].eEfsActivation[eef_id]) { + Eigen::Matrix3d rot = mDynStateSeq.dynamicsStateSequence[time_id].eEfsOrientation[eef_id].toRotationMatrix(); + Eigen::Vector3d rx = rot.col(0); + Eigen::Vector3d ry = rot.col(1); + + LinExpr lx, ly, lz, fx, fy, fz; + lx = LinExpr(mDynStateSeq.dynamicsStateSequence[time_id].eEfsPosition[eef_id].x()) - LinExpr(mVars[mCom.id(0,time_id)]) + LinExpr(mVars[mCopLocal[eef_id].id(0,mDynStateSeq.dynamicsStateSequence[time_id].eEfsActivationIds[eef_id])])*rx(0) + LinExpr(mVars[mCopLocal[eef_id].id(1,mDynStateSeq.dynamicsStateSequence[time_id].eEfsActivationIds[eef_id])])*ry(0); + ly = LinExpr(mDynStateSeq.dynamicsStateSequence[time_id].eEfsPosition[eef_id].y()) - LinExpr(mVars[mCom.id(1,time_id)]) + LinExpr(mVars[mCopLocal[eef_id].id(0,mDynStateSeq.dynamicsStateSequence[time_id].eEfsActivationIds[eef_id])])*rx(1) + LinExpr(mVars[mCopLocal[eef_id].id(1,mDynStateSeq.dynamicsStateSequence[time_id].eEfsActivationIds[eef_id])])*ry(1); + lz = LinExpr(mDynStateSeq.dynamicsStateSequence[time_id].eEfsPosition[eef_id].z()) - LinExpr(mVars[mCom.id(2,time_id)]) + LinExpr(mVars[mCopLocal[eef_id].id(0,mDynStateSeq.dynamicsStateSequence[time_id].eEfsActivationIds[eef_id])])*rx(2) + LinExpr(mVars[mCopLocal[eef_id].id(1,mDynStateSeq.dynamicsStateSequence[time_id].eEfsActivationIds[eef_id])])*ry(2); + fx = mVars[mFrcWorld[eef_id].id(0,mDynStateSeq.dynamicsStateSequence[time_id].eEfsActivationIds[eef_id])]; + fy = mVars[mFrcWorld[eef_id].id(1,mDynStateSeq.dynamicsStateSequence[time_id].eEfsActivationIds[eef_id])]; + fz = mVars[mFrcWorld[eef_id].id(2,mDynStateSeq.dynamicsStateSequence[time_id].eEfsActivationIds[eef_id])]; + + double lx_val, ly_val, lz_val, fx_val, fy_val, fz_val; + lx_val = mDynStateSeq.dynamicsStateSequence[time_id].eEfsPosition[eef_id].x() - mDynStateSeq.dynamicsStateSequence[time_id].com.x() + rx(0)*mDynStateSeq.dynamicsStateSequence[time_id].eEfsCop[eef_id].x() + ry(0)*mDynStateSeq.dynamicsStateSequence[time_id].eEfsCop[eef_id].y(); + ly_val = mDynStateSeq.dynamicsStateSequence[time_id].eEfsPosition[eef_id].y() - mDynStateSeq.dynamicsStateSequence[time_id].com.y() + rx(1)*mDynStateSeq.dynamicsStateSequence[time_id].eEfsCop[eef_id].x() + ry(1)*mDynStateSeq.dynamicsStateSequence[time_id].eEfsCop[eef_id].y(); + lz_val = mDynStateSeq.dynamicsStateSequence[time_id].eEfsPosition[eef_id].z() - mDynStateSeq.dynamicsStateSequence[time_id].com.z() + rx(2)*mDynStateSeq.dynamicsStateSequence[time_id].eEfsCop[eef_id].x() + ry(2)*mDynStateSeq.dynamicsStateSequence[time_id].eEfsCop[eef_id].y(); + fx_val = mDynStateSeq.dynamicsStateSequence[time_id].eEfsFrc[eef_id].x(); + fy_val = mDynStateSeq.dynamicsStateSequence[time_id].eEfsFrc[eef_id].y(); + fz_val = mDynStateSeq.dynamicsStateSequence[time_id].eEfsFrc[eef_id].z(); + + LinExpr ub_x, lb_x, ub_y, lb_y, ub_z, lb_z; + ub_x = ( LinExpr(std::pow(-lz_val+fy_val, 2.0) + std::pow( ly_val+fz_val, 2.0)) + ((LinExpr()-lz+fy)-(-lz_val+fy_val))*2.0*(-lz_val+fy_val) + (( ly+fz)-( ly_val+fz_val))*2.0*( ly_val+fz_val) ) - LinExpr(mVars[mUbVar[eef_id].id(0,mDynStateSeq.dynamicsStateSequence[time_id].eEfsActivationIds[eef_id])]); + lb_x = ( LinExpr(std::pow(-lz_val-fy_val, 2.0) + std::pow( ly_val-fz_val, 2.0)) + ((LinExpr()-lz-fy)-(-lz_val-fy_val))*2.0*(-lz_val-fy_val) + (( ly-fz)-( ly_val-fz_val))*2.0*( ly_val-fz_val) ) - LinExpr(mVars[mLbVar[eef_id].id(0,mDynStateSeq.dynamicsStateSequence[time_id].eEfsActivationIds[eef_id])]); + ub_y = ( LinExpr(std::pow( lz_val+fx_val, 2.0) + std::pow(-lx_val+fz_val, 2.0)) + (( lz+fx)-( lz_val+fx_val))*2.0*( lz_val+fx_val) + ((LinExpr()-lx+fz)-(-lx_val+fz_val))*2.0*(-lx_val+fz_val) ) - LinExpr(mVars[mUbVar[eef_id].id(1,mDynStateSeq.dynamicsStateSequence[time_id].eEfsActivationIds[eef_id])]); + lb_y = ( LinExpr(std::pow( lz_val-fx_val, 2.0) + std::pow(-lx_val-fz_val, 2.0)) + (( lz-fx)-( lz_val-fx_val))*2.0*( lz_val-fx_val) + ((LinExpr()-lx-fz)-(-lx_val-fz_val))*2.0*(-lx_val-fz_val) ) - LinExpr(mVars[mLbVar[eef_id].id(1,mDynStateSeq.dynamicsStateSequence[time_id].eEfsActivationIds[eef_id])]); + ub_z = ( LinExpr(std::pow(-ly_val+fx_val, 2.0) + std::pow( lx_val+fy_val, 2.0)) + ((LinExpr()-ly+fx)-(-ly_val+fx_val))*2.0*(-ly_val+fx_val) + (( lx+fy)-( lx_val+fy_val))*2.0*( lx_val+fy_val) ) - LinExpr(mVars[mUbVar[eef_id].id(2,mDynStateSeq.dynamicsStateSequence[time_id].eEfsActivationIds[eef_id])]); + lb_z = ( LinExpr(std::pow(-ly_val-fx_val, 2.0) + std::pow( lx_val-fy_val, 2.0)) + ((LinExpr()-ly-fx)-(-ly_val-fx_val))*2.0*(-ly_val-fx_val) + (( lx-fy)-( lx_val-fy_val))*2.0*( lx_val-fy_val) ) - LinExpr(mVars[mLbVar[eef_id].id(2,mDynStateSeq.dynamicsStateSequence[time_id].eEfsActivationIds[eef_id])]); + + double w_soft_constraint = mModel.getStgs().get(SolverDoubleParam_SoftConstraintWeight); + mQuadObjective.addQuaTerm(w_soft_constraint, ub_x); + mQuadObjective.addQuaTerm(w_soft_constraint, lb_x); + mQuadObjective.addQuaTerm(w_soft_constraint, ub_y); + mQuadObjective.addQuaTerm(w_soft_constraint, lb_y); + mQuadObjective.addQuaTerm(w_soft_constraint, ub_z); + mQuadObjective.addQuaTerm(w_soft_constraint, lb_z); + } + } + } + } + + mModel.setObjective(mQuadObjective, 0.0); + + // constant time horizon with time adaptation + if (mCentParam->isTimeHorizonFixed && mCentParam->heuristic == Heuristic::timeOptimization) { + mLinCons = 0.0; + for (int time_id=0; time_idnumTimeSteps; time_id++) + mLinCons += mVars[mDt.id(0,time_id)]; + mModel.addLinConstr(mLinCons, "=", mCentParam->numTimeSteps*mCentParam->timeStep); + } + + // upper and lower bounds constraints + for (int time_id=0; time_idnumTimeSteps; time_id++) { + if (mCentParam->heuristic == Heuristic::timeOptimization) { + mModel.addLinConstr(mVars[mDt.id(0,time_id)], ">", mCentParam->timeRange[0]); + mModel.addLinConstr(mVars[mDt.id(0,time_id)], "<", mCentParam->timeRange[1]); + } + for (int eef_id=0; eef_idnumActEEfs; eef_id++) { + if (mDynStateSeq.dynamicsStateSequence[time_id].eEfsActivation[eef_id]) { + mModel.addLinConstr(mVars[mTrqLocal[eef_id].id(0,mDynStateSeq.dynamicsStateSequence[time_id].eEfsActivationIds[eef_id])], ">", mCentParam->torqueRange[0]); + mModel.addLinConstr(mVars[mTrqLocal[eef_id].id(0,mDynStateSeq.dynamicsStateSequence[time_id].eEfsActivationIds[eef_id])], "<", mCentParam->torqueRange[1]); + mModel.addLinConstr(mVars[mCopLocal[eef_id].id(0,mDynStateSeq.dynamicsStateSequence[time_id].eEfsActivationIds[eef_id])], ">", mCentParam->copRange[eef_id][0]); + mModel.addLinConstr(mVars[mCopLocal[eef_id].id(0,mDynStateSeq.dynamicsStateSequence[time_id].eEfsActivationIds[eef_id])], "<", mCentParam->copRange[eef_id][1]); + mModel.addLinConstr(mVars[mCopLocal[eef_id].id(1,mDynStateSeq.dynamicsStateSequence[time_id].eEfsActivationIds[eef_id])], ">", mCentParam->copRange[eef_id][2]); + mModel.addLinConstr(mVars[mCopLocal[eef_id].id(1,mDynStateSeq.dynamicsStateSequence[time_id].eEfsActivationIds[eef_id])], "<", mCentParam->copRange[eef_id][3]); + } + } + } + + // friction cone constraints + for (int time_id=0; time_idnumTimeSteps; time_id++) { + for (int eef_id=0; eef_idnumActEEfs; eef_id++) { + if (mDynStateSeq.dynamicsStateSequence[time_id].eEfsContactType[eef_id] != ContactType::FullContact) { + if (mDynStateSeq.dynamicsStateSequence[time_id].eEfsActivation[eef_id]) { + Eigen::Matrix3d eff_rotation = mDynStateSeq.dynamicsStateSequence[time_id].eEfsOrientation[eef_id].toRotationMatrix(); + if (mCentParam->isFrictionConeLinear) { // using a linear representation + Eigen::Matrix rotated_mConeMatrix = mConeMatrix*eff_rotation.transpose(); + for (int row_id=0; row_id<4; row_id++) { + mLinCons = 0.0; + for (int axis_id=0; axis_id<3; axis_id++) + mLinCons += LinExpr(mVars[mFrcWorld[eef_id].id(axis_id,mDynStateSeq.dynamicsStateSequence[time_id].eEfsActivationIds[eef_id])])*rotated_mConeMatrix(row_id,axis_id); + mModel.addLinConstr(mLinCons, "<", 0.0); + } + } else { // using a second-order cone representation + LinExpr fx = 0.0, fy = 0.0, fz = 0.0; + for (int axis_id=0; axis_id<3; axis_id++) { + fx += LinExpr(mVars[mFrcWorld[eef_id].id(axis_id,mDynStateSeq.dynamicsStateSequence[time_id].eEfsActivationIds[eef_id])])*eff_rotation(axis_id,0); + fy += LinExpr(mVars[mFrcWorld[eef_id].id(axis_id,mDynStateSeq.dynamicsStateSequence[time_id].eEfsActivationIds[eef_id])])*eff_rotation(axis_id,1); + fz += LinExpr(mVars[mFrcWorld[eef_id].id(axis_id,mDynStateSeq.dynamicsStateSequence[time_id].eEfsActivationIds[eef_id])])*eff_rotation(axis_id,2); + } + mQuadCons.clear(); mQuadCons.addQuaTerm(1.0, fx); mQuadCons.addQuaTerm(1.0, fy); + mModel.addSocConstr(mQuadCons, "<", fz*mCentParam->frictionCoeff); + } + } + } + } + } + + for (int time_id=0; time_idnumTimeSteps; time_id++) { + if (mCentParam->heuristic == Heuristic::timeOptimization) { + if (is_first_time) { + // center of mass constraint + for (int axis_id=0; axis_id<3; axis_id++) { + if (time_id==0) { mLinCons = LinExpr(mVars[mCom.id(axis_id,time_id)]) - LinExpr(mCentParam->initialState.com[axis_id]) - LinExpr(mVars[mLMom.id(axis_id,time_id)])*(mDynStateSeq.dynamicsStateSequence[time_id].time/mCentParam->robotMass); } + else { mLinCons = LinExpr(mVars[mCom.id(axis_id,time_id)]) - LinExpr(mVars[mCom.id(axis_id,time_id-1)]) - LinExpr(mVars[mLMom.id(axis_id,time_id)])*(mDynStateSeq.dynamicsStateSequence[time_id].time/mCentParam->robotMass); } + mModel.addLinConstr(mLinCons, "=", 0.0); + } + + // linear momentum constraint + for (int axis_id=0; axis_id<3; axis_id++) { + if (time_id==0) { mLinCons = LinExpr(mVars[mLMom.id(axis_id,time_id)]) - LinExpr(mCentParam->initialState.lMom[axis_id]) - LinExpr(mVars[mLMomD.id(axis_id,time_id)])*mDynStateSeq.dynamicsStateSequence[time_id].time; } + else { mLinCons = LinExpr(mVars[mLMom.id(axis_id,time_id)]) - LinExpr(mVars[mLMom.id(axis_id,time_id-1)]) - LinExpr(mVars[mLMomD.id(axis_id,time_id)])*mDynStateSeq.dynamicsStateSequence[time_id].time; } + mModel.addLinConstr(mLinCons, "=", 0.0); + } + + // angular momentum constraint + for (int axis_id=0; axis_id<3; axis_id++) { + if (time_id==0) { mLinCons = LinExpr(mVars[mAMom.id(axis_id,time_id)]) - LinExpr(mCentParam->initialState.aMom[axis_id]) - LinExpr(mVars[mAMomD.id(axis_id,time_id)])*mDynStateSeq.dynamicsStateSequence[time_id].time; } + else { mLinCons = LinExpr(mVars[mAMom.id(axis_id,time_id)]) - LinExpr(mVars[mAMom.id(axis_id,time_id-1)]) - LinExpr(mVars[mAMomD.id(axis_id,time_id)])*mDynStateSeq.dynamicsStateSequence[time_id].time; } + mModel.addLinConstr(mLinCons, "=", 0.0); + } + } else { + LinExpr lmom, lmomd, amomd, dt = mVars[mDt.id(0,time_id)]; + double lmom_val, lmomd_val, amomd_val, dt_val = mDynStateSeq.dynamicsStateSequence[time_id].time; + + // center of mass constraint + for (int axis_id=0; axis_id<3; axis_id++) { + lmom = mVars[mLMom.id(axis_id,time_id)]; + lmom_val = mDynStateSeq.dynamicsStateSequence[time_id].lMom[axis_id]; + mLinCons = 0.0; + if (time_id==0) { mLinCons += LinExpr(mCentParam->initialState.com[axis_id]) - LinExpr(mVars[mCom.id(axis_id,time_id)]); } + else { mLinCons += LinExpr(mVars[mCom.id(axis_id,time_id-1)]) - LinExpr(mVars[mCom.id(axis_id,time_id)]); } + mLinCons += ( LinExpr(std::pow(dt_val+lmom_val, 2.0)) + ((dt+lmom)-(dt_val+lmom_val))*2.0*(dt_val+lmom_val) )*(0.25/mCentParam->robotMass) + - ( LinExpr(std::pow(dt_val-lmom_val, 2.0)) + ((dt-lmom)-(dt_val-lmom_val))*2.0*(dt_val-lmom_val) )*(0.25/mCentParam->robotMass); + mModel.addLinConstr(mLinCons, "=", 0.0); + } + + // linear momentum constraint + for (int axis_id=0; axis_id<3; axis_id++) { + lmomd = mVars[mLMomD.id(axis_id,time_id)]; + lmomd_val = mDynStateSeq.dynamicsStateSequence[time_id].lMomD[axis_id]; + mLinCons = 0.0; + if (time_id==0) { mLinCons += LinExpr(mCentParam->initialState.lMom[axis_id]) - LinExpr(mVars[mLMom.id(axis_id,time_id)]); } + else { mLinCons += LinExpr(mVars[mLMom.id(axis_id,time_id-1)]) - LinExpr(mVars[mLMom.id(axis_id,time_id)]); } + mLinCons += ( LinExpr(std::pow(dt_val+lmomd_val, 2.0)) + ((dt+lmomd)-(dt_val+lmomd_val))*2.0*(dt_val+lmomd_val) )*0.25 + - ( LinExpr(std::pow(dt_val-lmomd_val, 2.0)) + ((dt-lmomd)-(dt_val-lmomd_val))*2.0*(dt_val-lmomd_val) )*0.25; + mModel.addLinConstr(mLinCons, "=", 0.0); + } + + // angular momentum constraint + for (int axis_id=0; axis_id<3; axis_id++) { + amomd = mVars[mAMomD.id(axis_id,time_id)]; + amomd_val = mDynStateSeq.dynamicsStateSequence[time_id].aMomD[axis_id]; + mLinCons = 0.0; + if (time_id==0) { mLinCons += LinExpr(mCentParam->initialState.aMom[axis_id]) - LinExpr(mVars[mAMom.id(axis_id,time_id)]); } + else { mLinCons += LinExpr(mVars[mAMom.id(axis_id,time_id-1)]) - LinExpr(mVars[mAMom.id(axis_id,time_id)]); } + mLinCons += ( LinExpr(std::pow(dt_val+amomd_val, 2.0)) + ((dt+amomd)-(dt_val+amomd_val))*2.0*(dt_val+amomd_val) )*0.25 + - ( LinExpr(std::pow(dt_val-amomd_val, 2.0)) + ((dt-amomd)-(dt_val-amomd_val))*2.0*(dt_val-amomd_val) )*0.25; + mModel.addLinConstr(mLinCons, "=", 0.0); + } + } + + // linear momentum rate constraint + for (int axis_id=0; axis_id<3; axis_id++) { + mLinCons = LinExpr(mCentParam->robotMass*mCentParam->gravityVector[axis_id]) - LinExpr(mVars[mLMomD.id(axis_id,time_id)]); + for (int eef_id=0; eef_id < mCentParam->numActEEfs; eef_id++) + if (mDynStateSeq.dynamicsStateSequence[time_id].eEfsActivation[eef_id]) { mLinCons += LinExpr(mVars[mFrcWorld[eef_id].id(axis_id,mDynStateSeq.dynamicsStateSequence[time_id].eEfsActivationIds[eef_id])])*mCentParam->massTimesGravity; } + mModel.addLinConstr(mLinCons, "=", 0.0); + } + + // angular momentum rate constraint + for (int axis_id=0; axis_id<3; axis_id++) { + mLinCons = LinExpr() - LinExpr(mVars[mAMomD.id(axis_id,time_id)]); + for (int eef_id=0; eef_idnumActEEfs; eef_id++) { + if (mDynStateSeq.dynamicsStateSequence[time_id].eEfsActivation[eef_id]) { + Eigen::Matrix3d rot = mDynStateSeq.dynamicsStateSequence[time_id].eEfsOrientation[eef_id].toRotationMatrix(); + mLinCons += (LinExpr(mVars[mUbVar[eef_id].id(axis_id,mDynStateSeq.dynamicsStateSequence[time_id].eEfsActivationIds[eef_id])])-LinExpr(mVars[mLbVar[eef_id].id(axis_id,mDynStateSeq.dynamicsStateSequence[time_id].eEfsActivationIds[eef_id])]))*0.25*mCentParam->massTimesGravity; + mLinCons += LinExpr(mVars[mTrqLocal[eef_id].id(0,mDynStateSeq.dynamicsStateSequence[time_id].eEfsActivationIds[eef_id])])*rot.col(2)[axis_id]; + } + } + mModel.addLinConstr(mLinCons, "=", 0.0); + } + } else { + // center of mass constraint + for (int axis_id=0; axis_id<3; axis_id++) { + if (time_id==0) { mLinCons = LinExpr(mVars[mCom.id(axis_id,time_id)]) - LinExpr(mCentParam->initialState.com[axis_id]) - LinExpr(mVars[mLMom.id(axis_id,time_id)])*(mDynStateSeq.dynamicsStateSequence[time_id].time/mCentParam->robotMass); } + else { mLinCons = LinExpr(mVars[mCom.id(axis_id,time_id)]) - LinExpr(mVars[mCom.id(axis_id,time_id-1)]) - LinExpr(mVars[mLMom.id(axis_id,time_id)])*(mDynStateSeq.dynamicsStateSequence[time_id].time/mCentParam->robotMass); } + mModel.addLinConstr(mLinCons, "=", 0.0); + } + + // linear momentum constraint + for (int axis_id=0; axis_id<3; axis_id++) { + if (time_id==0) { mLinCons = LinExpr(mVars[mLMom.id(axis_id,time_id)]) - LinExpr(mDynStateSeq.dynamicsStateSequence[time_id].time*(mCentParam->robotMass*mCentParam->gravityVector[axis_id])) - LinExpr(mCentParam->initialState.lMom(axis_id)); } + else { mLinCons = LinExpr(mVars[mLMom.id(axis_id,time_id)]) - LinExpr(mDynStateSeq.dynamicsStateSequence[time_id].time*(mCentParam->robotMass*mCentParam->gravityVector[axis_id])) - LinExpr(mVars[mLMom.id(axis_id,time_id-1)]); } + + for (int eef_id=0; eef_idnumActEEfs; eef_id++) + if (mDynStateSeq.dynamicsStateSequence[time_id].eEfsActivation[eef_id]) { mLinCons += LinExpr(mVars[mFrcWorld[eef_id].id(axis_id,mDynStateSeq.dynamicsStateSequence[time_id].eEfsActivationIds[eef_id])])*(-mDynStateSeq.dynamicsStateSequence[time_id].time*mCentParam->massTimesGravity); } + if (time_id==0) { mLinCons += LinExpr(mCentParam->externalForce[axis_id])*(-mDynStateSeq.dynamicsStateSequence[time_id].time*mCentParam->massTimesGravity); } + mModel.addLinConstr(mLinCons, "=", 0.0); + } + + // angular momentum constraint + for (int axis_id=0; axis_id<3; axis_id++) { + if (time_id==0) { mLinCons = LinExpr(mVars[mAMom.id(axis_id,time_id)]) - LinExpr(mCentParam->initialState.aMom[axis_id]); } + else { mLinCons = LinExpr(mVars[mAMom.id(axis_id,time_id)]) - LinExpr(mVars[mAMom.id(axis_id,time_id-1)]); } + + for (int eef_id=0; eef_idnumActEEfs; eef_id++) { + if (mDynStateSeq.dynamicsStateSequence[time_id].eEfsActivation[eef_id]) { + mLinCons += (LinExpr(mVars[mLbVar[eef_id].id(axis_id,mDynStateSeq.dynamicsStateSequence[time_id].eEfsActivationIds[eef_id])])-LinExpr(mVars[mUbVar[eef_id].id(axis_id,mDynStateSeq.dynamicsStateSequence[time_id].eEfsActivationIds[eef_id])]))*(0.25*mDynStateSeq.dynamicsStateSequence[time_id].time*mCentParam->massTimesGravity); + + Eigen::Vector3d rz = mDynStateSeq.dynamicsStateSequence[time_id].eEfsOrientation[eef_id].toRotationMatrix().col(2); + mLinCons += LinExpr(mVars[mTrqLocal[eef_id].id(0,mDynStateSeq.dynamicsStateSequence[time_id].eEfsActivationIds[eef_id])])*(-rz[axis_id]*mDynStateSeq.dynamicsStateSequence[time_id].time); + } + } + mModel.addLinConstr(mLinCons, "=", 0.0); + } + } + } + + QuadConstrApprox qapprox = QuadConstrApprox::None; + switch (mCentParam->heuristic) { + case Heuristic::timeOptimization: { qapprox = QuadConstrApprox::None; break; } + case Heuristic::trustRegion: { qapprox = QuadConstrApprox::TrustRegion; break; } + case Heuristic::softConstraint: { qapprox = QuadConstrApprox::SoftConstraint; break; } + default: { qapprox = QuadConstrApprox::None; break; } + } + + for (int time_id=0; time_idnumTimeSteps; time_id++) { + for (int eef_id=0; eef_idnumActEEfs; eef_id++) { + if (mDynStateSeq.dynamicsStateSequence[time_id].eEfsActivation[eef_id]) { + Eigen::Matrix3d rot = mDynStateSeq.dynamicsStateSequence[time_id].eEfsOrientation[eef_id].toRotationMatrix(); + Eigen::Vector3d rx = rot.col(0); + Eigen::Vector3d ry = rot.col(1); + + LinExpr lx, ly , lz, fx, fy, fz; + lx = LinExpr(mDynStateSeq.dynamicsStateSequence[time_id].eEfsPosition[eef_id].x()) - LinExpr(mVars[mCom.id(0,time_id)]) + LinExpr(mVars[mCopLocal[eef_id].id(0,mDynStateSeq.dynamicsStateSequence[time_id].eEfsActivationIds[eef_id])])*rx(0) + LinExpr(mVars[mCopLocal[eef_id].id(1,mDynStateSeq.dynamicsStateSequence[time_id].eEfsActivationIds[eef_id])])*ry(0); + ly = LinExpr(mDynStateSeq.dynamicsStateSequence[time_id].eEfsPosition[eef_id].y()) - LinExpr(mVars[mCom.id(1,time_id)]) + LinExpr(mVars[mCopLocal[eef_id].id(0,mDynStateSeq.dynamicsStateSequence[time_id].eEfsActivationIds[eef_id])])*rx(1) + LinExpr(mVars[mCopLocal[eef_id].id(1,mDynStateSeq.dynamicsStateSequence[time_id].eEfsActivationIds[eef_id])])*ry(1); + lz = LinExpr(mDynStateSeq.dynamicsStateSequence[time_id].eEfsPosition[eef_id].z()) - LinExpr(mVars[mCom.id(2,time_id)]) + LinExpr(mVars[mCopLocal[eef_id].id(0,mDynStateSeq.dynamicsStateSequence[time_id].eEfsActivationIds[eef_id])])*rx(2) + LinExpr(mVars[mCopLocal[eef_id].id(1,mDynStateSeq.dynamicsStateSequence[time_id].eEfsActivationIds[eef_id])])*ry(2); + fx = mVars[mFrcWorld[eef_id].id(0,mDynStateSeq.dynamicsStateSequence[time_id].eEfsActivationIds[eef_id])]; + fy = mVars[mFrcWorld[eef_id].id(1,mDynStateSeq.dynamicsStateSequence[time_id].eEfsActivationIds[eef_id])]; + fz = mVars[mFrcWorld[eef_id].id(2,mDynStateSeq.dynamicsStateSequence[time_id].eEfsActivationIds[eef_id])]; + + // upper and lower bounds on momentum rate constraints + mQuadCons.clear(); mQuadCons.addQuaTerm(1.0, LinExpr()-lz+fy); mQuadCons.addQuaTerm(1.0, ly+fz); + mModel.addQuaConstr(mQuadCons, "<", mVars[mUbVar[eef_id].id(0,mDynStateSeq.dynamicsStateSequence[time_id].eEfsActivationIds[eef_id])], qapprox); + + mQuadCons.clear(); mQuadCons.addQuaTerm(1.0, lz+fx); mQuadCons.addQuaTerm(1.0, LinExpr()-lx+fz); + mModel.addQuaConstr(mQuadCons, "<", mVars[mUbVar[eef_id].id(1,mDynStateSeq.dynamicsStateSequence[time_id].eEfsActivationIds[eef_id])], qapprox); + + mQuadCons.clear(); mQuadCons.addQuaTerm(1.0, LinExpr()-ly+fx); mQuadCons.addQuaTerm(1.0, lx+fy); + mModel.addQuaConstr(mQuadCons, "<", mVars[mUbVar[eef_id].id(2,mDynStateSeq.dynamicsStateSequence[time_id].eEfsActivationIds[eef_id])], qapprox); + + mQuadCons.clear(); mQuadCons.addQuaTerm(1.0, LinExpr()-lz-fy); mQuadCons.addQuaTerm(1.0, ly-fz); + mModel.addQuaConstr(mQuadCons, "<", mVars[mLbVar[eef_id].id(0,mDynStateSeq.dynamicsStateSequence[time_id].eEfsActivationIds[eef_id])], qapprox); + + mQuadCons.clear(); mQuadCons.addQuaTerm(1.0, lz-fx); mQuadCons.addQuaTerm(1.0, LinExpr()-lx-fz); + mModel.addQuaConstr(mQuadCons, "<", mVars[mLbVar[eef_id].id(1,mDynStateSeq.dynamicsStateSequence[time_id].eEfsActivationIds[eef_id])], qapprox); + + mQuadCons.clear(); mQuadCons.addQuaTerm(1.0, LinExpr()-ly-fx); mQuadCons.addQuaTerm(1.0, lx-fy); + mModel.addQuaConstr(mQuadCons, "<", mVars[mLbVar[eef_id].id(2,mDynStateSeq.dynamicsStateSequence[time_id].eEfsActivationIds[eef_id])], qapprox); + + // end-effector length constraint + mQuadCons.clear(); mQuadCons.addQuaTerm(1.0, lx-mCentParam->eEfOffset[eef_id].x()); mQuadCons.addQuaTerm(1.0, ly-mCentParam->eEfOffset[eef_id].y()); mQuadCons.addQuaTerm(1.0, lz-mCentParam->eEfOffset[eef_id].z()); + mModel.addQuaConstr(mQuadCons, "<", pow(mCentParam->maxEEfLengths[eef_id],2.0)); + } + } + } + + // formulate problem in standard conic form and solve it + mTimer.start(); + mExitCode = mModel.optimize(); + mSolveTime += mTimer.stop(); + + // extract solution + mSolution.resize(mNumVars, 1); mSolution.setZero(); + for (int var_id=0; var_idheuristic == Heuristic::timeOptimization) + _saveSolution(mDt); + _saveSolution(mCom); + _saveSolution(mAMom); + _saveSolution(mLMom); + if (mCentParam->heuristic == Heuristic::timeOptimization) { + _saveSolution(mLMomD); + _saveSolution(mAMomD); + } + for (int eef_id=0; eef_idnumActEEfs; eef_id++) { + _saveSolution(mFrcWorld[eef_id]); + _saveSolution(mCopLocal[eef_id]); + _saveSolution(mTrqLocal[eef_id]); + } + + // computation of linear momentum out of forces + Eigen::Vector3d frc, len, trq; + Eigen::MatrixXd compos(3,mCentParam->numTimeSteps); compos.setZero(); + Eigen::MatrixXd linmom(3,mCentParam->numTimeSteps); linmom.setZero(); + Eigen::MatrixXd angmom(3,mCentParam->numTimeSteps); angmom.setZero(); + + if (mCentParam->heuristic == Heuristic::timeOptimization) { + for (int time_id=0; time_idnumTimeSteps; time_id++) { + if (time_id==0) { + compos.col(time_id) = mCentParam->initialState.com; + linmom.col(time_id) = mCentParam->initialState.lMom; + } else { + compos.col(time_id) = compos.col(time_id-1); + linmom.col(time_id) = linmom.col(time_id-1); + } + + linmom.col(time_id) += mCentParam->robotMass * mCentParam->gravityVector*mVars[mDt.id(0,time_id)].get(SolverDoubleParam_X); + for (int eef_id=0; eef_idnumActEEfs; eef_id++) { + if (mDynStateSeq.dynamicsStateSequence[time_id].eEfsActivation[eef_id]) { + frc.x() = mCentParam->massTimesGravity*mVars[mFrcWorld[eef_id].id(0,mDynStateSeq.dynamicsStateSequence[time_id].eEfsActivationIds[eef_id])].get(SolverDoubleParam_X); + frc.y() = mCentParam->massTimesGravity*mVars[mFrcWorld[eef_id].id(1,mDynStateSeq.dynamicsStateSequence[time_id].eEfsActivationIds[eef_id])].get(SolverDoubleParam_X); + frc.z() = mCentParam->massTimesGravity*mVars[mFrcWorld[eef_id].id(2,mDynStateSeq.dynamicsStateSequence[time_id].eEfsActivationIds[eef_id])].get(SolverDoubleParam_X); + linmom.col(time_id).head(3) += mVars[mDt.id(0,time_id)].get(SolverDoubleParam_X)*frc; + } + } + compos.col(time_id).head(3) += 1.0/mCentParam->robotMass*mVars[mDt.id(0,time_id)].get(SolverDoubleParam_X)*linmom.col(time_id).head(3); + } + } + + // computation of angular momentum out of forces and lengths + for (int time_id=0; time_idnumTimeSteps; time_id++) { + if (time_id == 0) { angmom.col(time_id) = mCentParam->initialState.aMom; } + else { angmom.col(time_id) = angmom.col(time_id-1); } + + for (int eef_id=0; eef_id < mCentParam->numActEEfs; eef_id++) { + if (mDynStateSeq.dynamicsStateSequence[time_id].eEfsActivation[eef_id]) { + Eigen::Matrix3d rot = mDynStateSeq.dynamicsStateSequence[time_id].eEfsOrientation[eef_id].toRotationMatrix(); + Eigen::Vector3d rx = rot.col(0); + Eigen::Vector3d ry = rot.col(1); + Eigen::Vector3d rz = rot.col(2); + + if (mCentParam->heuristic == Heuristic::timeOptimization) { + len.x() = mDynStateSeq.dynamicsStateSequence[time_id].eEfsPosition[eef_id].x() - compos(0,time_id) + rx(0)*mVars[mCopLocal[eef_id].id(0,mDynStateSeq.dynamicsStateSequence[time_id].eEfsActivationIds[eef_id])].get(SolverDoubleParam_X) + ry(0)*mVars[mCopLocal[eef_id].id(1,mDynStateSeq.dynamicsStateSequence[time_id].eEfsActivationIds[eef_id])].get(SolverDoubleParam_X); + len.y() = mDynStateSeq.dynamicsStateSequence[time_id].eEfsPosition[eef_id].y() - compos(1,time_id) + rx(1)*mVars[mCopLocal[eef_id].id(0,mDynStateSeq.dynamicsStateSequence[time_id].eEfsActivationIds[eef_id])].get(SolverDoubleParam_X) + ry(1)*mVars[mCopLocal[eef_id].id(1,mDynStateSeq.dynamicsStateSequence[time_id].eEfsActivationIds[eef_id])].get(SolverDoubleParam_X); + len.z() = mDynStateSeq.dynamicsStateSequence[time_id].eEfsPosition[eef_id].z() - compos(2,time_id) + rx(2)*mVars[mCopLocal[eef_id].id(0,mDynStateSeq.dynamicsStateSequence[time_id].eEfsActivationIds[eef_id])].get(SolverDoubleParam_X) + ry(2)*mVars[mCopLocal[eef_id].id(1,mDynStateSeq.dynamicsStateSequence[time_id].eEfsActivationIds[eef_id])].get(SolverDoubleParam_X); + } else { + len.x() = mDynStateSeq.dynamicsStateSequence[time_id].eEfsPosition[eef_id].x() - mVars[mCom.id(0,time_id)].get(SolverDoubleParam_X) + rx(0)*mVars[mCopLocal[eef_id].id(0,mDynStateSeq.dynamicsStateSequence[time_id].eEfsActivationIds[eef_id])].get(SolverDoubleParam_X) + ry(0)*mVars[mCopLocal[eef_id].id(1,mDynStateSeq.dynamicsStateSequence[time_id].eEfsActivationIds[eef_id])].get(SolverDoubleParam_X); + len.y() = mDynStateSeq.dynamicsStateSequence[time_id].eEfsPosition[eef_id].y() - mVars[mCom.id(1,time_id)].get(SolverDoubleParam_X) + rx(1)*mVars[mCopLocal[eef_id].id(0,mDynStateSeq.dynamicsStateSequence[time_id].eEfsActivationIds[eef_id])].get(SolverDoubleParam_X) + ry(1)*mVars[mCopLocal[eef_id].id(1,mDynStateSeq.dynamicsStateSequence[time_id].eEfsActivationIds[eef_id])].get(SolverDoubleParam_X); + len.z() = mDynStateSeq.dynamicsStateSequence[time_id].eEfsPosition[eef_id].z() - mVars[mCom.id(2,time_id)].get(SolverDoubleParam_X) + rx(2)*mVars[mCopLocal[eef_id].id(0,mDynStateSeq.dynamicsStateSequence[time_id].eEfsActivationIds[eef_id])].get(SolverDoubleParam_X) + ry(2)*mVars[mCopLocal[eef_id].id(1,mDynStateSeq.dynamicsStateSequence[time_id].eEfsActivationIds[eef_id])].get(SolverDoubleParam_X); + } + frc.x() = mCentParam->massTimesGravity*mVars[mFrcWorld[eef_id].id(0,mDynStateSeq.dynamicsStateSequence[time_id].eEfsActivationIds[eef_id])].get(SolverDoubleParam_X); + frc.y() = mCentParam->massTimesGravity*mVars[mFrcWorld[eef_id].id(1,mDynStateSeq.dynamicsStateSequence[time_id].eEfsActivationIds[eef_id])].get(SolverDoubleParam_X); + frc.z() = mCentParam->massTimesGravity*mVars[mFrcWorld[eef_id].id(2,mDynStateSeq.dynamicsStateSequence[time_id].eEfsActivationIds[eef_id])].get(SolverDoubleParam_X); + trq = rz * mVars[mTrqLocal[eef_id].id(0,mDynStateSeq.dynamicsStateSequence[time_id].eEfsActivationIds[eef_id])].get(SolverDoubleParam_X); + angmom.col(time_id).head(3) += mDynStateSeq.dynamicsStateSequence[time_id].time*(len.cross(frc)+trq); + } + } + } + if (mCentParam->heuristic != Heuristic::timeOptimization) { mAMom.setGuessValue(angmom); } + _storeSolution(); + + if (mCentParam->heuristic == Heuristic::timeOptimization) { + // computing convergence error + if (is_first_time) { mLastConvergenceErr = SolverSetting::inf; } + else { mLastConvergenceErr = mConvergenceErr; } + mCom.getGuessValue(mComGuess); double com_err = (mComGuess-compos).norm()/mCentParam->numTimeSteps; + mLMom.getGuessValue(mLMomGuess); double lmom_err = (mLMomGuess-linmom).norm()/mCentParam->numTimeSteps; + mAMom.getGuessValue(mAMomGuess); double amom_err = (mAMomGuess-angmom).norm()/mCentParam->numTimeSteps; + mConvergenceErr = std::max(com_err, std::max(lmom_err, amom_err)); + + if (mConvergenceErr < mCentParam->maxTimeResidualTolerance) { mHasConverged = true; } + if (std::abs(mLastConvergenceErr-mConvergenceErr) < mCentParam->minTimeResidualImprovement) { mHasConverged = true; } + if (mHasConverged) { mAMom.setGuessValue(angmom); _storeSolution(); } + } +} + +void CentroidPlanner::_saveToFile(const DynamicsStateSequence& _ref_sequence) { + if (mCentParam->isStoreData) { + try + { + YAML::Node cfg_pars = YAML::LoadFile(mCentParam->cfgFile.c_str()); + + YAML::Node qcqp_cfg; + qcqp_cfg["dynopt_params"]["time_step"] = mCentParam->timeStep; + qcqp_cfg["dynopt_params"]["end_com"] = mComPosGoal; + qcqp_cfg["dynopt_params"]["robot_mass"] = mCentParam->robotMass; + qcqp_cfg["dynopt_params"]["n_act_eefs"] = mCentParam->numActEEfs; + qcqp_cfg["dynopt_params"]["ini_com"] = mCentParam->initialState.com; + qcqp_cfg["dynopt_params"]["time_horizon"] = mCentParam->timeHorizon; + qcqp_cfg["cntopt_params"] = cfg_pars["contact_plan"]; + + mCom.getGuessValue(mMatGuess); qcqp_cfg["dynopt_params"]["com_motion"] = mMatGuess; + mLMom.getGuessValue(mMatGuess); qcqp_cfg["dynopt_params"]["lin_mom"] = mMatGuess; + mAMom.getGuessValue(mMatGuess); qcqp_cfg["dynopt_params"]["ang_mom"] = mMatGuess; + + // building momentum references + for (int time_id=0; time_idnumTimeSteps; time_id++) { mMatGuess.col(time_id) = _ref_sequence.dynamicsStateSequence[time_id].com; } qcqp_cfg["dynopt_params"]["com_motion_ref"] = mMatGuess; + for (int time_id=0; time_idnumTimeSteps; time_id++) { mMatGuess.col(time_id) = _ref_sequence.dynamicsStateSequence[time_id].lMom; } qcqp_cfg["dynopt_params"]["lin_mom_ref"] = mMatGuess; + for (int time_id=0; time_idnumTimeSteps; time_id++) { mMatGuess.col(time_id) = _ref_sequence.dynamicsStateSequence[time_id].aMom; } qcqp_cfg["dynopt_params"]["ang_mom_ref"] = mMatGuess; + + // saving vector of time-steps + mMatGuess.resize(1, mCentParam->numTimeSteps); mMatGuess.setZero(); + mMatGuess(0,0) = mDynStateSeq.dynamicsStateSequence[0].time; + for (int time_id=1; time_idnumTimeSteps; time_id++) + mMatGuess(0,time_id) = mMatGuess(0,time_id-1) + mDynStateSeq.dynamicsStateSequence[time_id].time; + qcqp_cfg["dynopt_params"]["time_vec"] = mMatGuess; + + // saving vector of forces, torques and cops + for (int eef_id=0; eef_idnumActEEfs; eef_id++) { + mMatGuess.resize(3, mCentParam->numTimeSteps); mMatGuess.setZero(); + for (int time_id=0; time_idnumTimeSteps; time_id++) + if (mDynStateSeq.dynamicsStateSequence[time_id].eEfsActivation[eef_id]) + mMatGuess.col(time_id).head(3) = mDynStateSeq.dynamicsStateSequence[time_id].eEfsFrc[eef_id]; + qcqp_cfg["dynopt_params"]["eef_frc_"+std::to_string(eef_id)] = mMatGuess; + + mMatGuess.resize(1, mCentParam->numTimeSteps); mMatGuess.setZero(); + for (int time_id=0; time_idnumTimeSteps; time_id++) + if (mDynStateSeq.dynamicsStateSequence[time_id].eEfsActivation[eef_id]) + mMatGuess(0, time_id) = mDynStateSeq.dynamicsStateSequence[time_id].eEfsTrq[eef_id].z(); + qcqp_cfg["dynopt_params"]["eef_trq_"+std::to_string(eef_id)] = mMatGuess; + + mCopLocal[eef_id].getGuessValue(mMatGuess); + qcqp_cfg["dynopt_params"]["eef_cop_"+std::to_string(eef_id)] = mMatGuess; + } + std::ofstream file_out(mCentParam->saveDynamicsFile); file_out << qcqp_cfg; + } + catch (YAML::ParserException &e) { std::cout << e.what() << "\n"; } + } +} + +void CentroidPlanner::_saveSolution(solver::OptimizationVariable& opt_var) { + mMatGuess.resize(opt_var.getNumRows(), opt_var.getNumCols()); mMatGuess.setZero(); + for (int col_id=0; col_idnumTimeSteps; time_id++) + mDynStateSeq.dynamicsStateSequence[time_id].com = Eigen::Vector3d(mMatGuess.block<3,1>(0,time_id)); + + mLMom.getGuessValue(mMatGuess); + for (int time_id=0; time_idnumTimeSteps; time_id++) + mDynStateSeq.dynamicsStateSequence[time_id].lMom = Eigen::Vector3d(mMatGuess.block<3,1>(0,time_id)); + + mAMom.getGuessValue(mMatGuess); + for (int time_id=0; time_idnumTimeSteps; time_id++) + mDynStateSeq.dynamicsStateSequence[time_id].aMom = Eigen::Vector3d(mMatGuess.block<3,1>(0,time_id)); + + if (mCentParam->heuristic == Heuristic::timeOptimization) { + mDt.getGuessValue(mMatGuess); + for (int time=0; timenumTimeSteps; time++) + mDynStateSeq.dynamicsStateSequence[time].time = mMatGuess(0,time); + + mLMomD.getGuessValue(mMatGuess); + for (int time_id=0; time_idnumTimeSteps; time_id++) + mDynStateSeq.dynamicsStateSequence[time_id].lMomD = Eigen::Vector3d(mMatGuess.block<3,1>(0,time_id)); + + mAMomD.getGuessValue(mMatGuess); + for (int time_id=0; time_idnumTimeSteps; time_id++) + mDynStateSeq.dynamicsStateSequence[time_id].aMomD = Eigen::Vector3d(mMatGuess.block<3,1>(0,time_id)); + } + + for (int eef_id=0; eef_idnumActEEfs; eef_id++) { + mFrcWorld[eef_id].getGuessValue(mMatGuess); + for (int time_id=0; time_idnumTimeSteps; time_id++) + if (mDynStateSeq.dynamicsStateSequence[time_id].eEfsActivation[eef_id]) + mDynStateSeq.dynamicsStateSequence[time_id].eEfsFrc[eef_id] = Eigen::Vector3d(mMatGuess.block<3,1>(0,mDynStateSeq.dynamicsStateSequence[time_id].eEfsActivationIds[eef_id])); + + mCopLocal[eef_id].getGuessValue(mMatGuess); + for (int time_id=0; time_idnumTimeSteps; time_id++) + if (mDynStateSeq.dynamicsStateSequence[time_id].eEfsActivation[eef_id]) + mDynStateSeq.dynamicsStateSequence[time_id].eEfsCop[eef_id] = Eigen::Vector3d(mMatGuess(0,mDynStateSeq.dynamicsStateSequence[time_id].eEfsActivationIds[eef_id]), mMatGuess(1,mDynStateSeq.dynamicsStateSequence[time_id].eEfsActivationIds[eef_id]), 0.0); + + mTrqLocal[eef_id].getGuessValue(mMatGuess); + for (int time_id=0; time_idnumTimeSteps; time_id++) + if (mDynStateSeq.dynamicsStateSequence[time_id].eEfsActivation[eef_id]) + mDynStateSeq.dynamicsStateSequence[time_id].eEfsTrq[eef_id] = Eigen::Vector3d(0.0, 0.0, mMatGuess(0,mDynStateSeq.dynamicsStateSequence[time_id].eEfsActivationIds[eef_id])); + } +} + +void CentroidPlanner::_addVariableToModel(const OptimizationVariable& opt_var, Model& model, std::vector& vars) +{ + opt_var.getValues(mMatLb, mMatUb, mMatGuess, mSize, mVariableType); + for (int col_id=0; col_id ConeMat; + void getCone(double fcoeff, FrictionCone::ConeMat& cone_mat) + { + if (fcoeff <= 0.001) { fcoeff = 0.001; } + Eigen::Vector3d fconevec = Eigen::Vector3d(0.5*sqrt(2.0), 0.5*sqrt(2.0), -fcoeff); fconevec.normalize(); + Eigen::Matrix3d fconemat = Eigen::Matrix3d::Identity(); + double angle = 2*M_PI/4; + fconemat(0,0) = cos(angle); fconemat(0,1) = -sin(angle); + fconemat(1,0) = sin(angle); fconemat(1,1) = cos(angle); + for (int i=0; i<4; i++) { + cone_mat.row(i) = fconevec; + fconevec = fconemat * fconevec; + } + } +}; + enum class Heuristic { trustRegion, softConstraint, timeOptimization }; class CentroidPlannerParameter : public PlannerParameter { public: - CentroidPlannerParameter (){}; + CentroidPlannerParameter () : PlannerParameter() {}; virtual ~CentroidPlannerParameter (){}; void paramSetFromYaml(const std::string & configFile_); @@ -61,10 +89,126 @@ class CentroidPlanner : public Planner virtual ~CentroidPlanner (); private: - /* data */ virtual void _doPlan(); virtual void _evalTrajectory( double time, Eigen::VectorXd & pos, Eigen::VectorXd & vel, Eigen::VectorXd & trq); + + std::shared_ptr mCentParam; + + /** + * class that stores all information about the problem, interfaces between high level definition + * of the problem and its corresponding mathematical construction to solve it with a solver instance + */ + solver::Model mModel; + + /*! helper variables to construct linear and quadratic expressions */ + solver::LinExpr mLinCons; + solver::DCPQuadExpr mQuadObjective, mQuadCons; + + /*! exit code of the optimization problem */ + solver::ExitCode mExitCode; + + /** + * c++ vector containing all problem variables defined by the user. Does not include extra + * variables required to write the problem in standard conic form. + */ + std::vector mVars; + + /** + * initial configuration of the robot, including center of mass position, linear and angular momenta, + * end-effectors configurations: activation, position, orientation + */ + DynamicsState mIniState; + + /*! simple helper class to build a linear approximation of a friction cone */ + FrictionCone mFrictionCone; + FrictionCone::ConeMat mConeMatrix; + + /** + * dynamics sequence of dynamic states. This is the main interface between the user + * and the planner. The user can find in this variable all the optimization results + */ + DynamicsStateSequence mDynStateSeq; + + /*! clock for timing purposes */ + Clock mTimer; + + /*! type for optimization variable: continuous 'C' or binary 'B' and type of heuristic */ + char mVariableType; + + /*! helper boolean variables for the optimization problem */ + bool mHasConverged; + + /*! helper integer variables for the optimization problem */ + int mSize, mNumVars; + + /*! helper double variables for the optimization problem */ + double mSolveTime, mConvergenceErr, mLastConvergenceErr; + + /*! helper vector variables for the optimization problem */ + Eigen::Vector3d mComPosGoal; + + /*! helper optimization variables for the optimization problem */ + solver::OptimizationVariable mDt, mCom, mLMom, mAMom, mLMomD, mAMomD; + std::array mFrcWorld, mTrqLocal, mCopLocal, mUbVar, mLbVar; + + /*! helper matrices and vectors for the optimization problem */ + solver::OptimizationVariable::OptVector mSolution; + solver::OptimizationVariable::OptMatrix mMatLb, mMatUb, mMatGuess, mComGuess, mLMomGuess, mAMomGuess; + + /** + * function to initialize and configure the optimization + * @param[in] param_ planning parameter + */ + void _initialize(); + + /** + * function to add each variable to the Model and assign a unique identifier + * to it, to be used by the optimizer to construct the problem + * @param[in] opt_var helper optimization variable for model predictive control + * @param[in] model instance of solver interface to collect constraints, objective and solve problem + * @param[in] vars vector of optimization variables + */ + void _addVariableToModel(const OptimizationVariable& opt_var, + Model& model, + std::vector& vars); + + /** + * function to parse equations and objective into optimization problem and attempt to find a solution + * @param[in] ref_sequence dynamics sequence to be used as momentum tracking reference (can be zeros). + * @param[in] update_tracking_objective changes weights from regulation to tracking of momentum in ref_sequence + * @return ExitCode flag that indicates the optimization result (for example: optimal, infeasible) + */ + solver::ExitCode _optimize(bool update_tracking_objective = false); + + /** + * function to initialize optimization variables: type [continuous or binary], + * guess value [if any], upper and lower bounds for the variable + */ + void _initializeOptimizationVariables(); + + void _updateTrackingObjective(); + + /** + * functions to update tracking objective for momentum from penalty to tracking + * and attempt to find a solution to the optimization problem + * @param[in] ref_sequence dynamics sequence to be used as momentum tracking reference (can be zeros). + * @param[in] is_first_time flag to indicate if this is the first time the solution is being constructed + */ + void _internalOptimize(bool is_first_time = false); + + /** + * functions that transfers optimal solution from model to helper class OptimizationVariable, + * from helper class OptimizationVariable to dynamics sequence to be accessed by the user, and + * from dynamics sequence to a file. + * @param[in] opt_var helper optimization variable for model predictive control + * @param[in] ref_sequence dynamics sequence to be used as momentum tracking reference (can be zeros). + */ + void _saveSolution(solver::OptimizationVariable& opt_var); + void _storeSolution(); + void _saveToFile(const DynamicsStateSequence& ref_sequence); }; + + diff --git a/PnC/CentroidPlanner/ContactPlanInterface.hpp b/PnC/CentroidPlanner/ContactPlanInterface.hpp index 844737fb..e2fe61e7 100644 --- a/PnC/CentroidPlanner/ContactPlanInterface.hpp +++ b/PnC/CentroidPlanner/ContactPlanInterface.hpp @@ -20,24 +20,24 @@ class ContactPlanInterface double timeStep) { // configuration of end-effectors during contact dynamics_sequence.activeEEfSteps.setZero(); - for (int eff_id=0; eff_id=contactSequence.eEfContacts[eff_id][cnt_id].timeIni && - current_time< contactSequence.eEfContacts[eff_id][cnt_id].timeEnd) + if (current_time>=contactSequence.eEfContacts[eef_id][cnt_id].timeIni && + current_time< contactSequence.eEfContacts[eef_id][cnt_id].timeEnd) { - dynamics_sequence.dynamicsStateSequence[time_id].eEfsActivation[eff_id] = true; - dynamics_sequence.dynamicsStateSequence[time_id].eEfsIds[eff_id] = counter++; - dynamics_sequence.dynamicsStateSequence[time_id].eEfsContactType[eff_id] = contactSequence.eEfContacts[eff_id][cnt_id].contactType; - dynamics_sequence.dynamicsStateSequence[time_id].eEfsPosition[eff_id] = contactSequence.eEfContacts[eff_id][cnt_id].position; - dynamics_sequence.dynamicsStateSequence[time_id].eEfsOrientation[eff_id] = contactSequence.eEfContacts[eff_id][cnt_id].orientation; + dynamics_sequence.dynamicsStateSequence[time_id].eEfsActivation[eef_id] = true; + dynamics_sequence.dynamicsStateSequence[time_id].eEfsActivationIds[eef_id] = counter++; + dynamics_sequence.dynamicsStateSequence[time_id].eEfsContactType[eef_id] = contactSequence.eEfContacts[eef_id][cnt_id].contactType; + dynamics_sequence.dynamicsStateSequence[time_id].eEfsPosition[eef_id] = contactSequence.eEfContacts[eef_id][cnt_id].position; + dynamics_sequence.dynamicsStateSequence[time_id].eEfsOrientation[eef_id] = contactSequence.eEfContacts[eef_id][cnt_id].orientation; } } } - dynamics_sequence.activeEEfSteps[eff_id] = counter; + dynamics_sequence.activeEEfSteps[eef_id] = counter; } } diff --git a/PnC/CentroidPlanner/DynamicsState.hpp b/PnC/CentroidPlanner/DynamicsState.hpp index 043c1789..fffdbe7a 100644 --- a/PnC/CentroidPlanner/DynamicsState.hpp +++ b/PnC/CentroidPlanner/DynamicsState.hpp @@ -25,7 +25,7 @@ class DynamicsState aMomD(Eigen::VectorXd::Zero(3)), lMomD(Eigen::VectorXd::Zero(3)) { for (int eff = 0; eff < CentroidModel::numEEf; ++eff) { - eEfsIds[eff] = 0; + eEfsActivationIds[eff] = 0; eEfsFrc[eff] = Eigen::Vector3d::Zero(); eEfsTrq[eff] = Eigen::Vector3d::Zero(); eEfsCop[eff] = Eigen::Vector3d::Zero(); @@ -63,7 +63,7 @@ class DynamicsState double time; Eigen::Vector3d com, aMom, lMom, aMomD, lMomD; - IntArray eEfsIds; + IntArray eEfsActivationIds; BoolArray eEfsActivation; OriArray eEfsOrientation; CntTypeArray eEfsContactType; diff --git a/PnC/DracoPnC/CMakeLists.txt b/PnC/DracoPnC/CMakeLists.txt index eaec5770..f2051ff6 100644 --- a/PnC/DracoPnC/CMakeLists.txt +++ b/PnC/DracoPnC/CMakeLists.txt @@ -5,7 +5,8 @@ target_link_libraries(DracoPnC drake::drake ${DART_LIBRARIES} myRobotSystem myUtils - myWBC) + myWBC + myCentPlanner) add_executable(gen_draco_offline_traj OfflineTrajGen/src.cpp) target_link_libraries(gen_draco_offline_traj DracoPnC) diff --git a/PnC/DracoPnC/OfflineTrajGen/src.cpp b/PnC/DracoPnC/OfflineTrajGen/src.cpp index 11894628..a4c1381b 100644 --- a/PnC/DracoPnC/OfflineTrajGen/src.cpp +++ b/PnC/DracoPnC/OfflineTrajGen/src.cpp @@ -1,7 +1,22 @@ #include +#include "CentroidPlanner.hpp" +#include +#include "Configuration.h" int main(int argc, char *argv[]) { + std::shared_ptr mParam = + std::make_shared(); + mParam->paramSetFromYaml(THIS_COM"Config/Draco/CENTROID_PLANNER.yaml"); + + std::unique_ptr mPlanner = + std::make_unique(); + + mPlanner->updatePlanningParameter(mParam); + double time(1.0); + Eigen::VectorXd pos, vel, trq; + mPlanner->getPlan(time, pos, vel, trq); + std::cout << "Done" << std::endl; return 0; } diff --git a/README.md b/README.md index 2bbaac1d..68c7ca5e 100644 --- a/README.md +++ b/README.md @@ -10,7 +10,7 @@ dynamics algorithm in [Dart](https://github.com/junhyeokahn/dart). - Direct Transcription - Direct Collocation - [Constrained Direct Collocation](https://github.com/DAIRLab/dairlib-public) -- [Embedded Conic Solver](https://github.com/embotech/ecos) using [sparse matrix routines](http://www.suitesparse.com) +- [Embedded Conic Solver](https://github.com/embotech/ecos) using [Sparse Matrix Routines](http://www.suitesparse.com) ### Trajectory Planning - [Centroid Dyanimcs Planning for Humanoid](https://arxiv.org/pdf/1709.09265.pdf) diff --git a/Utils/Clock.hpp b/Utils/Clock.hpp new file mode 100644 index 00000000..8d6f5c73 --- /dev/null +++ b/Utils/Clock.hpp @@ -0,0 +1,20 @@ +#pragma once + +#include + +class Clock { + public: + Clock(){} + ~Clock(){} + + void start() { ini_time_ = std::chrono::high_resolution_clock::now(); } + double stop() { + end_time_ = std::chrono::high_resolution_clock::now(); + duration_ = std::chrono::duration_cast(end_time_-ini_time_); + return double(duration_.count())*1e-3; + } + + private: + std::chrono::microseconds duration_; + std::chrono::high_resolution_clock::time_point ini_time_, end_time_; +};